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This  work  addresses  the  kinematic  and  geometric  questions  associated  with 
a robot  manipulator  moving  in  an  environment  having  obstacles  or  adjacent 
manipulators.  The  kinematic  and  geometric  properties  of  a robot  manipulator  are 
analyzed  in  terms  of  its  articulation  which  is  used  to  describe  the  capacity  of  the 
robot  manipulator  to  continuously  change  its  configuration  while  its  end  effector 
follows  a prespecified  path  with  free  orientation. 

In  this  research,  the  articulation  of  spatial  robots  with  special  geometries  such 
as  the  PUMA  and  TRS  robots  is  analyzed  by  determining  the  permissible,  allowable 
and  favorable  orientations  of  robot  links,  which  are  used  to  describe  motion 
capability  of  the  robot  links.  The  permissible  orientations  are  determined  by  the 
flexure  of  the  robot  with  its  end  effector  pinned  at  some  intermediate  target  position 
and  can  be  obtained  by  analyzing  the  workspace  and  dexterity  geometry.  The 
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allowable  orientations  of  a specific  link  of  the  manipulator  direct  the  link  avoiding 
interference  with  obstacles  or  the  links  of  an  adjacent  robot  manipulator.  They  are 
obtained  by  finding  the  intersection  of  some  geometric  figures  such  as  line  segments, 
circles,  conics,  cones,  and  cylinders.  The  favorable  orientations  of  a link  are 
contained  in  its  allowable  orientations  and  are  consistent  with  all  links  avoiding 
interference.  They  are  determined  by  the  kinematic  constraints  of  the  robot  links. 

This  analysis  gives  a range  of  favorable  orientations  which  can  be  used  to  plan 
an  interference-free  motion  of  a robot  manipulator  to  avoid  spherical  and  polyhedral 
obstacles  or  an  adjacent  robot  manipulator. 

A time  scheduling  scheme  of  two  robot  manipulators  moving  in  a coordinated 
manner  is  also  presented.  A potential  collision  region  is  obtained  by  detecting 
collisions  along  rectilinear  trajectories  of  the  manipulators  using  a sphere  model  for 
the  end  effector.  An  interference-free  motion  is  then  obtained  by  modifying  the 
trajectory  of  one  of  the  manipulators  using  time  delay  or  speed  reduction  to  avoid 
collision. 
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CHAPTER  1 
INTRODUCTION 


In  recent  years  a scientific  basis  for  robotics  has  been  evolving  which  will 
lead  to  superior  motion  planning  capabilities.  This  scientific  basis  comprises  of  the 
knowledge  and  understanding  of  some  principal  aspects  of  robotics  such  as  robot 
geometry  and  kinematics,  workspace,  dexterity,  special  configurations,  obstacle 
avoidance  and  coordination  of  multi-robot  systems. 

A fundamental  problem  in  motion  planning  is  to  determine  whether  a 
prespecified  motion  is  possible  or  not.  This  problem  can  be  couched  in  geometric 
terms  and  constitutes  a proper  basis  for  providing  the  robot  with  intelligence  to 
improve  and  speed  up  its  decision-making  capability. 

In  the  works  of  Krishnamurthy  (1985)  and  Lipkin  et  al.  (1985),  the 
manipulator  workspace  geometry  is  used  to  develop  time  efficient  algorithms  which 
determine  whether  rectilinear  motions  by  a planar  3R  manipulator  can  be 
successfully  executed.  The  end  effector  orientations  are  either  prescribed  or  left 
unspecified  along  the  path.  Shieh  and  Duffy  ( 1989a, b)  extended  the  algorithm  to 
plan  a collision  free  rectilinear  motion  of  the  end  effector  of  a planar  3R 
manipulator  with  a single  circular  obstacle  inside  the  workspace. 
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This  research  is  to  apply  a theory  of  articulation  of  robot  manipulators  to 
study  collision  detection  and  avoidance  problems  of  a robot  manipulator  with 
obstacles  inside  the  workspace  and  coordinated  motions  of  two  manipulators 
operating  in  parallel.  The  term  "articulation"  appeared  first  in  the  works  of  Young 
and  Duffy  (1987a,  1987b).  Choi(1990)  then  applied  the  concept  of  articulation  and 
a geometry  of  interference  to  develop  a blue  print  algorithm  for  path  planning  of 
spatial  robot  manipulators. 

Articulation  is  used  to  describe  the  capacity  of  a manipulator  to  continuously 
change  its  configuration  such  that  it  moves  with  a snake-like  motion.  The  problems 
of  interest  here  are  the  determination  of  possible  execution  of  various  types  of  tasks 
and  the  generation  of  a sequence  of  motions  to  achieve  the  tasks  within  an 
obstacles-strewn  environment. 

In  this  study,  the  links  of  a robot  manipulator  are  modeled  at  first  by  line 
segments  and  then  extended  to  solids  such  as  parallelepipeds  and  cylinders.  Since 
most  objects  can  be  properly  approximated  by  polyhedrons  or  spheres  or  their 
combinations,  obstacles  are  exemplified  by  polyhedrons  or  spheres.  An  interference 
avoidance  strategy  is  to  be  developed  which  guarantees  that  a robot  manipulator  will 
arrive  safely  at  its  destination  without  interference  by  articulating  its  links  while 
tracking  specified  trajectories  or  by  rescheduling  the  trajectory  of  the  robot 
manipulator.  It  is  assumed  that  the  positions  and  orientations  of  the  robot 
manipulators  at  the  beginning  and  end  points  are  specified.  In  addition,  the 
trajectory  of  the  robot  manipulator  which  connects  the  beginning  and  end  points  is 
also  defined. 
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The  end  effector  of  the  robot  manipulator  is  considered  to  be  moving  on  the 
specified  trajectory  for  a specific  task  regardless  of  orientation  which  is  considered 
to  be  a free  parameter,  and  which  together  with  the  remaining  link  orientations  is 
used  to  flex  and  hence  articulate  the  arm  to  avoid  interference.  In  other  words, 
positional  constraints  of  end  effectors  are  imposed  on  the  motion  planning  of  robot 
manipulators.  Geometric  dexterity  which  describes  a range  of  motion  of  a robot  end 
effector  about  a point  (Lovell  1983)  can  be  applied  to  facilitate  the  analysis. 

This  analysis  takes  into  consideration,  firstly,  the  boundaries  of  the  robot 
workspace  which  pose  a primary  constraint  on  the  motion  of  the  robot  manipulator. 
This  is  followed  by  the  consideration  of  intersections  of  various  geometric  objects 
which  are  used  to  model  the  links  of  robot  manipulators  and  obstacles  in  the  work 
environment.  In  this  study,  three  sets  of  words,  viz.,  permissible  orientations, 
allowable  orientations,  and  favorable  orientations,  are  used  to  describe  the  robot 
link’s  orientations.  The  permissible  orientations  of  a link  are  determined  by  the 
flexure  with  the  end  effector  pinned  at  some  intermediate  target  position  on  a 
trajectory.  The  allowable  orientations  of  a particular  link  of  a manipulator  are 
determined  by  the  relative  locations  of  the  links  and  the  obstacles.  It  should  be 
noted  that  the  remaining  links  of  the  manipulator  may  well  interfere  with  the 
obstacles.  The  favorable  orientations  of  a link  of  a manipulator  are  contained  in  its 
allowable  orientations  and  are  consistent  with  all  links  avoiding  interference. 

As  a result,  a sequence  of  joint  angles  for  the  robot  manipulators  can  be 
determined,  and  these  joint  angles  satisfy  the  constraints  imposed  by  the  specified 
trajectory  and  the  presence  of  obstacles.  It  should  be  noted  that  path  refers  to  the 
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space  curve  traced  by  the  end  effector.  However,  trajectory  also  includes  velocity 
and  acceleration  information. 

Another  interesting  problem  to  be  investigated  here  is  the  trajectory  planning 
of  robot  manipulators  for  the  purpose  of  coordinating  movement  of  multi-robot 
operation.  This  problem  is  arisen  from  the  possibility  that  one  robot  manipulator 
may  not  be  able  to  arrive  at  the  goal  point  without  colliding  with  the  other 
manipulator  while  tracking  the  specified  trajectory.  In  this  situation,  modification 
of  the  trajectory  is  suggested.  This  investigation  is  also  useful  for  the  motion 
planning  of  robot  manipulators  when  they  work  in  an  environment  where  obstacles 
are  movable. 

The  design  of  a motion  planning  system  to  coordinate  actions  of  multiple 
robot  manipulators  within  a shared  environment  has  attracted  increasing  research 
activity  in  recent  years.  The  motions  of  multiple  robots  may  be  classified  into  two 
modes  by  means  of  their  task  descriptions:  (1)  A tightly  coordinated  mode  refers  to 
the  motion  such  that  the  robot  manipulators  are  collaborating  to  execute  a common 
task  function.  (2)  A loosely  coordinated  mode  refers  to  the  motion  such  that  the 
robot  manipulators  are  executing  unrelated  tasks  independently  (Chien  et  al.  1988). 

In  this  research,  the  robot  manipulators  are  considered  to  be  executing  in  the 
loosely  coordinated  mode.  Every  manipulator  in  the  system  may  consider  another 
manipulator  in  the  common  workspace  as  a nonstationary  obstacle. 

In  the  work  of  Freund  and  Hoyer  (1986),  an  approach  was  presented  for  the 
solution  of  the  findpath  problem  in  multi-robot  systems.  A hierarchical  coordinator 
is  designed  for  real-time  collision  avoidance.  The  coalition  avoidance  strategy  is 
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based  on  an  analytically  described  trajectory  that  serves  for  collision  detection  as 
well  as  avoidance.  Each  robot  is  assigned  an  order  of  precedence.  The  robot  with 
highest  precedence  has  the  right  of  way  and  every  robot  must  avoid  a collision  with 
robots  with  higher  precedence. 

Roach  and  Boaz  (1987)  proposed  a time/space  planning  system  to  coordinate 
the  actions  of  two  robot  manipulators  for  transfer  movements.  It  is  assumed  that  the 
path  of  motion  for  each  manipulator  has  been  determined  without  regard  to  spatial 
intersection.  The  planning  system  needs  to  check  one  manipulator’s  position  against 
the  other  manipulator’s  position.  The  strategy  for  the  collision  avoidance  is  by 
temporally  delaying  or  by  altering  the  path  of  one  arm. 

Other  research  on  collision-free  motion  planning  for  two  moving  robots  in  a 
common  workspace  can  be  found  in  the  works  of  Lee  and  Lee  (1987)  and  Basta  et 
al.  (1988).  In  their  works  the  wrist  of  a manipulator  is  modeled  as  a sphere  which 
includes  the  end  effector  and  the  workpiece  grasped  by  the  end  effector.  Collisions 
are  restricted  to  between  the  wrists  of  the  two  robots,  and  the  arm  collisions  are 
not  taken  into  account.  The  strategy  suggested  is  to  use  the  time  scheduling  concept, 
namely,  time  delay  or  speed  reduction  of  one  of  the  robots  to  avoid  potential 
collision. 

In  this  research,  the  methods  used  by  Lee  and  Lee  (1987)  and  Basta  et  al. 
(1988)  are  modified  and  a faster  trajectory  is  generated  for  one  of  the  robots  whose 
trajectory  is  changed  to  avoid  potential  collision. 

The  contents  of  this  dissertation  are  briefly  described  as  follows.  Chapter  2 
covers  the  detection  and  determination  of  intersection  of  some  geometric  objects 
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which  will  see  applications  in  later  chapters.  Chapter  3 introduces  the  computation 
of  permissible  orientations  of  robot  manipulators,  followed  by  the  computation  of 
allowable  orientations  for  the  purpose  of  avoiding  interference  and  concludes  with 
the  computation  of  favorable  orientations.  In  this  chapter,  the  robot  links  are 
modeled  as  line  segments.  Chapter  4 extends  the  line  segment  models  to  solid  link 
models,  accomplished  by  way  of  obstacles  expansion  and  intersection  of  solid  objects. 
Chapter  5 deals  with  the  coordination  of  two  robot  manipulators,  which  is 
accomplished  by  way  of  scheduling  the  motions  of  two  robots  via  time  delay  or  speed 
reduction  of  one  of  the  robots.  Chapter  6 presents  results  of  numerical  examples  of 
collision  avoidance  strategies  described  in  the  previous  chapters.  It  should  be  noted 
that  the  paths  considered  here  are  by  no  means  confined  to  rectilinear.  In  fact,  the 
analysis  in  this  work  can  be  applied  to  any  type  of  paths.  However,  due  to  the 
simplicity  of  rectilinear  paths,  they  are  used  as  examples.  Finally,  conclusions  and 
recommendations  for  future  work  are  given  in  Chapter  7. 


CHAPTER  2 

INTERSECTIONS  OF  SOME  GEOMETRIC  OBJECTS 


The  algorithm  for  planning  interference-free  motion  of  robot  manipulators 
contains  detection  and  determination  of  intersections  of  various  geometric  objects 
which  model  physical  elements,  e.g.  links,  of  robot  manipulators  or  perspective 
projections  and  planar  sections  of  these  physical  elements.  If  these  geometric  objects 
do  intersect,  we  must  be  able  to  mathematically  describe  the  intersection.  As  the 
complexity  of  objects  increases,  the  need  for  computational  subtlety  and  complexity 
increases. 

The  purpose  of  this  chapter  is  to  provide  mathematical  representations  of 
intersections  of  some  geometric  objects  used  to  model  physical  objects  in  this  work. 
In  addition,  some  geometric  transformations  that  may  assist  the  determination  of 
intersections  are  presented.  The  chapter  begins  with  the  simple  problem  of  finding 
the  points  of  intersection  between  a line  segment  and  other  geometric  objects  and 
concludes  with  the  more  subtle  and  complex  problem  of  finding  the  intersection 
between  a cone  and  a cylinder. 

2.1  Intersection  of  Two  Line  Segments 

Let  the  endponts  of  a pair  of  line  segments  AB  and  GH  be  represented  by 
A(xA,yA),  B(xB,yB)  and  G(xG,yG)  H(xH,yH),  and  the  equations  of  these  two  line 
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segments  in  parametric  forms  can  be  expressed  as  follows 

ab  : x = xA  + Sj(xA  - xB)  0 < Sj  < 1 

y = yA  + sx(yA  - yB) 


(2.1) 


GH  : x = Xq  + s2(xG  - xH)  0 < s2  < 1 (2.2) 

y = yG  + s2(yG  - yH) 

A direct  method  for  determining  the  intersection  of  these  two  line  segments 
is  to  compute  the  parameters  of  the  point  of  intersection  by  solving  Eqs.  (2.1)  and 
(2.2),  which  are  given  by 


Sl  = (AG,  DG)  / (AB,  HG)  (2.3) 

s2  = (AB,  AG)  / (AB,  HG)  (2.4) 


where 


(IJ,  MN)  = 


XJ  ‘ XI  XN  XM 


(2.5) 


yj  - yi  yN  - yM 

If  the  values  of  these  two  parameters  are  both  between  0 and  1,  the  two  line 
segments  intersect. 

An  affine  transformation  introduced  in  the  work  of  Young  and  Duffy  (1987a) 
can  also  be  applied  to  check  the  intersection  of  a pair  of  line  segments.  Briefly,  the 
relative  position  and  orientation  of  a pair  of  line  segments  in  a plane  are 
transformed  into  a point  in  a 3-dimensional  space.  The  3-dimensional  space  is  then 
converted  into  a 2-dimensional  plane  by  a parallel  perspective  projection.  In  the  2- 
dimensional  projection  plane,  a nonallowable  manifold  (NM)  is  determined  which 
is  used  to  check  the  interference  of  these  two  line  segments. 


The  coordinates  of  the  image  point  in  the  projection  plane  is  computed  by  the 
following  transformation  matrix, 
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X 

1 

-cot# 

X 

Y 

0 

-CSC# 

y 

(2.6) 


where  (x,y)  is  the  relative  position,  # is  the  relative  orientation,  and  (X,Y)  is  the 
coordinates  of  the  image  point  (see  Fig.  2.1).  The  advantage  of  this  transformation 
is  that  it  transforms  a sequence  of  relative  locations  of  a pair  of  line  segments  into 
a planar  curve.  The  condition  for  intersection  is  thus  determined  by  the  location  of 
the  curve. 

It  is  interesting  to  note  that  when  the  relative  position  of  a pair  of  line 
segments  is  fixed  and  their  relative  orientation  varies  from  0 to  27r,  the  resulting 
image  is  a hyperbola.  The  equation  of  the  hyperbola  can  be  expressed  as  follows 


Y2  - (X  - x)2  = y2 


(2.7) 


where  (x,y)  is  the  feed  relative  position.  In  this  perspective,  the  image  of  all  relative 
locations  of  two  line  segments  in  the  projection  plane  is  a set  of  hyperbolas. 
Analogously,  when  the  relative  orientation  of  two  line  segments  is  fixed  and  their 
relative  position  tracks  a circle,  the  resulting  image  becomes  an  ellipse,  a circle  or 
two  lines,  the  equation  of  which  is  given  by 

(X  - xc)2  - 2cos#(X  - xc)(Y  - yc)  + (Y  - yc)2  = rc2  (2.8) 


where  (xc,yc)  and  rc  are  the  center  and  the  radius  of  the  circle  that  the  relative 
position  of  the  two  line  segments  tracks,  and  # is  the  fixed  relative  orientation.  In 
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this  perspective,  the  image  of  all  relative  locations  of  two  line  segments  in  the 
projection  plane  is  a set  of  ellipses,  circles  and  lines. 

Furthermore,  it  is  helpful  to  investigate  the  situations  when  the  image  points 
are  on  the  boundary  of  (NM).  In  the  following,  the  lengths  of  the  two  line  segments 
are  represented  by  | AB  | and  | GH  | . The  possible  situations  for  the  image  points 
to  lie  on  the  boundary  of  (NM)  can  be  categorized  into  four  types  as  follows, 

(a)  The  image  point  is  at  X = 0 and  0 < Y < | AB  | . In  this  case,  the  line 
segment  ab  touches  the  endpoint  G of  the  line  segment  GH  (Fig.  2.2(a)). 

(b)  The  image  point  is  at  0 < X < | GH  | and  Y = | AB  | . In  this  case,  the 
endpoint  B of  the  line  segment  ab  is  on  the  line  segment  GH  (Fig.  2.2(b)). 

(c)  The  image  point  is  at  0 < X < | GH  | and  Y = 0.  In  this  case,  the  endpoint 
A of  the  line  segment  ab  is  on  the  line  segment  GH  (Fig.  2.2(c)). 

(d)  The  image  point  is  at  X = | GH  | and  0 < Y < | AB  | . In  this  case,  the  line 
segment  AB  touches  the  endpoint  H of  the  line  segment  GH  (Fig.  2.2(d)). 

2.2  Intersection  of  a Line  Segment  And  a Circle 

Suppose  the  equation  of  a line  segment  ab  is  represented  by  Eq.  (2.1)  and 
the  equation  of  a circle  with  center  C(xc,yc)  and  radius  rc  is  given  as  follows 

(x  - Xe)2  + (y  - yc)2  = rc2  (2.9) 

The  point  of  intersection  of  the  line  segment  and  the  circle  can  be  derived  by  solving 
Sj  from  Eqs.  (2.1)  and  (2.9).  Since  the  derived  equation  is  a quadratic  equation  in 
Sn  two  solutions  for  Sj  are  obtained  which  can  be  complex  conjugate,  distinct  real  or 
double  real  roots.  If  the  quadratic  equation  has  real  roots  between  0 and  1,  the  line 
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segment  and  the  circle  intersect,  and  Eq.  (2.1)  can  be  used  to  determine  the  point 
of  intersection. 

The  position  of  a line  segment  with  respect  to  a circle  can  be  used  to 
determine  the  intersection  of  a line  segment  and  a circle.  Suppose,  at  the  outset,  a 
fixed  world  coordinate  (xy)  system  is  specified.  Then,  a reference  coordinate  system 
(x’y’)  whose  +x  axis  is  the  direction  of  AB  and  whose  origin  is  the  center  of  the 
circle  is  selected.  Let  the  circle  be  denoted  by  C.  The  relative  position  of  the  line 
segment  ab  with  respect  to  the  circle  C is  represented  by  the  coordinates  of  one 
endpoint,  e.g.,  A(xa’,  yA’),  of  the  line  segment  in  the  reference  coordinate  system. 
Note  that  only  the  relative  position  of  a line  segment  and  a circle  is  defined,  relative 
orientation  is  not  difined. 

Let  the  orientation  of  the  line  segment  ab  with  respect  to  the  +x  axis  of  the 
world  coordinate  frame  be  denoted  by  $.  The  relative  position  (xA’,  yA’)  can  be 
determined  as  follows, 


XA’ 

cos$ 

sin<$ 

xA  - *c 

yA’ 

-sin# 

cos$ 

1 

U 

i 

1 

The  necessary  and  sufficient  condition  for  the  line  segment  AB  to  intersect 
the  circle  C is  that  the  endpoint  A of  the  line  segment  AB  lies  inside  a region  of 
intersection  (RI)  as  shown  in  Fig.  2.4.  In  other  words,  the  relative  position  is  inside 
(RI).  The  straight  parts  of  (RI)  are  parallel  to  the  line  segment  ab  and  with  length 
| AB  | . The  other  two  parts  of  (RI)  are  semicircles  with  radius  equal  to  that  of  the 
circle  C.  The  boundary  of  (RI)  can  be  expressed  by 
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y’  = ±(rc2  - X2)1/2,  0 < x’  < rc 
y’  = ±rc,  - 1 AB  | < x’  < 0 

y’  = ±[rc2  - (x*  + | AB  | )2]1/2,  - 1 AB  | - rc  < x’  < - 1 AB  | 


(2.11b) 


(2.11a) 


(2.11c) 


Further,  it  is  worthwhile  to  discuss  the  cases  when  the  relative  position  lies 
on  the  boundary  of  (RI).  Three  cases  can  be  categorized  as  follows: 

(a)  The  endpoint  A of  the  line  segment  AB  touches  the  circle  C.  The  relative 
position  lies  on  the  boundary  given  by  Eq.  (2.11a)  (see  Fig.  2.5(a)). 

(b)  The  endpoint  B of  the  line  segment  AB  touches  the  circle  C.  The  relative 
position  lies  on  the  boundary  given  by  Eq.  (2.11c)  (see  Fig.  2.5(b)). 

(c)  The  line  segment  AB  is  tangent  to  the  circle  C.  The  relative  position  lies  on 
the  boundary  given  by  Eq.  (2.11b)  (see  Fig.  2.5(c)). 

A point  can  be  considered  as  a line  segment  with  zero  length  or  a circle  with 
zero  radius.  In  this  perspective,  the  development  in  this  and  the  previous  sections 
can  also  be  used  to  transform  the  relative  position  of  a line  segment  with  respect 
to  a point.  However,  it  is  necessary  to  note  that  when  the  transformation  for  a pair 
of  line  segments  is  used,  the  relative  orientation  is  expressed  by  the  orientation  of 
the  line  segment  in  the  world  coordinate  system.  This  is  because  there  is  no  relative 
orientation  between  a line  segment  and  a point. 

When  the  transformation  for  a pair  of  line  segments  is  used  for  the 
transformation  of  a line  segment  and  a point,  the  nonallowable  manifold  in  the 
projection  plane  is  X = 0 and  0 < Y < | AB  | , where  | AB  | is  the  length  of  the  line 
segment.  In  contrast,  when  the  relative  position  of  a line  segment  and  a circle  is 
used,  the  region  of  intersection  is  - 1 AB  | < x’  < 0 and  y’  = 0. 
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2,3  Intersection  of  a Line  Segment  and  an  Ellipse 

Analogous  to  the  analysis  in  Section  2.2,  the  intersection  of  a line  segment  of 
Eq.  (2.1)  and  an  ellipse  of  the  form 

(x  - xE)2  (y  - yE)2 
+ = 1 


(2.12) 

a2  b2 

can  be  derived  by  computing  the  roots  of  a quadratic  equation.  Here  (xE,  yE)  is  the 
center  and  a,  b are  lengths  of  semi-major  and  semi-minor  axes,  respectively,  of  the 
ellipse. 

Alternatively,  a geometric  transformation  can  be  used  to  transform  an 
isothetic  ellipse  of  the  following  form 
x2  y2 


= 1 


a2 


(2.13) 


into  a circle  (Oommen  and  Reichstein  1987)(see  Fig.  2.6).  This  transformation 
scales  the  dimensions  along  the  major  and  minor  axes  of  the  ellipse  by  dividing  them 
by  ox  and  oy  respectively,  and  these  two  scale  factors  satisfy  the  following  relation 


ax  b 


(2.14) 


°y  a 


The  parametric  form  of  the  ellipse  after  this  transformation  becomes 

x = axacos0  (2.15a) 

y = aybsin9  (2.15b) 


For  convenience,  it  is  assumed  that  the  ellipse  is  transformed  into  a circle  of  radius 
R.  This  transformation,  being  linear,  maps  straight  lines  into  straight  lines.  The 
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slope  of  a line  in  a plane  is  denoted  by  m and  m’  is  the  slope  of  the  line  after  the 
transformation  becomes  (ay/crx)m. 

The  perpendicular  distance  between  two  parallel  lines  is  denoted  by  d and  d’ 
is  the  perpendicular  distance  between  the  two  transformed  parallel  lines  (see  Fig. 
2.7).  It  is  then  desirable  to  find  the  relation  between  d and  d’.  Let  E(xE,yE)  and 
F(xF,yF)  represent  respective  points  on  two  parallel  lines  /E  and  lF  of  slope  m,  and  the 


equations  of  the  two  parallel  lines  are  given  by 

lE  : y = mx  - mxE  + yE  (2.16) 

/F  : y = mx  - mxF  + yF  (2.17) 

The  perpendicular  distance  between  these  two  lines  is 

d = | m(xE  - xF)  - (yE  - yF)  | / (m2  + 1)1/2  (2.18) 

After  the  transformation,  the  equations  of  the  two  parallel  lines  become 

Ie  : y = m’x  - m’apcg  + ayyE  (2.19) 

/F  : y = m’x  - m’apq,  + ayyF  (2.20) 

and  the  perpendicular  distance  between  these  two  lines  is 

d’  = | m’ax(xE  - xF)  - ay(yE  - yF)  | / (m’2  + 1)1/2  (2.21) 

Replacing  m’  by  (ay/ax) m,  Eq.  (2.21)  can  be  expressed  as 

d’  = I x I K M m(xE  ' xf)  - (yE  * yF) I / (oy2m2  + a2)1'2 

= d|ax|  |ay|[(m2  + 1)  / (ay2m2  + a2)]1'2  (2.22) 


The  midpoint  of  a line  segment  is  preserved  under  this  transformation.  This 
can  be  realized  by  considering  that  the  midpoint  M of  a line  segment  with  end 
points  A(xA,yA)  and  B(xB,yB)  is  represented  by  ((xA  + xB)/2,(yA  + yB)/2).  Using  the 
transformation,  the  coordinates  of  M become  (ax(xA  + xB)/2,ay(yA  + yB)/2),  which  is  the 
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midpoint  of  (o^A,ayyA)  and  (axxB,ayyB),  the  coordinates  of  points  A and  B after  the 
transformation. 

This  transformation  has  been  applied  to  determine  the  interference  of  elliptic 
objects  (Oommen  and  Reichstein  1987).  In  this  study,  it  is  used  to  assist  the 
determination  of  interference  of  a rectangle  and  an  ellipse  and  can  be  found  in 
Section  4.2. 


2.4  Intersection  of  a Cone  and  a Sphere 

This  section  is  devoted  to  the  detection  and  determination  of  the  intersection 
of  a right  circular  cone  and  a sphere.  In  this  study,  a circular  cone  (CNt)  is  traced 
by  the  end  effector  link  of  a manipulator  with  a endpoint  fixed.  This  is  the  vertex 
of  the  cone  while  the  other  endpoint  of  the  end  effector  link  lies  on  a circle  (Cb) 
which  is  called  the  base  of  the  cone  and  the  plane  where  the  base  lies  is  called  the 
base  plane  of  the  cone  (see  Fig.  2.8).  The  base  plane  of  the  cone  CN,  is  denoted 
by  n.  The  orientation  of  the  link  in  which  it  intersects  a sphere  is  of  interest. 

The  detection  of  intersection  between  a right  circular  cone  and  a sphere  Sob 
can  be  accomplished  by  firstly  considering  the  plane  which  contains  the  axis  of  the 
cone  and  the  center  of  the  sphere  (assume  the  axis  of  the  cone  does  not  pass 
through  the  center  of  the  sphere).  This  plane  intersects  the  cone  in  an  isosceles 
triangle  and  intersects  the  sphere  in  a circle.  It  is  observed  that  when  the  circle 
intersects  either  one  of  the  two  equal  sides  (PA  or  PB)  of  the  triangle  (see  Fig.  2.9), 
the  cone  may  intersect  the  sphere. 
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In  order  to  find  the  intersection  of  the  cone  and  the  sphere  Sob,  the  polar 
plane  of  the  vertex  P of  the  cone  with  respect  to  the  sphere  Sob  is  obtained  (see  Fig. 
2.10).  This  polar  plane  intersects  the  sphere  Sob  in  a circle  Cs,  and  any  tangent  from 
P to  Sob  has  a point  contact  with  the  circle  Cs. 

A perspective  projection  is  then  used  to  project  the  circle  Cs  onto  the  base 
plane  of  the  cone.  The  image  of  the  circle  Cs  under  this  projection  is  a conic  which 
can  be  an  ellipse,  a hyperbola,  or  a parabola.  This  can  be  perceived  by  considering 
a plane  section  of  a second  right  circular  cone  CN2  whose  vertex  is  point  P and  all 
elements  of  this  cone  have  a point  contact  with  Cs. 

Figure  2.11  shows  the  circular  cone  CN2  and  a section  IVjJ  of  the  cone  by  the 
plane  n which  is  the  base  plane  of  CN2.  Here,  Vj  and  V2  are  the  points  of  the  curve 
IVjJ  on  the  intersection  of  n and  the  plane  perpendicular  to  n through  the  line  PG, 
where  G is  the  center  of  circle  Cs.  Let  G’  be  the  intersection  of  n and  PG,  and  C 
be  the  point  in  which  the  bisector  of  the  angle  G’ViP  meets  PG.  The  point  C is  at 
the  same  distance  r2  from  PVj  and  from  the  line  of  symmetry  Y^G’  of  the  curve  IV^; 
and  the  perpendicular  from  C upon  VjG’  is  normal  to  the  plane  n,  being  in  a plane 
perpendicular  to  n.  A sphere  of  radius  rx  can  be  generated  with  center  at  C.  Let 
Fj  denote  the  point  where  the  sphere  is  tangent  to  the  plane  of  the  section  and  B: 
denote  the  point  where  the  sphere  is  tangent  to  the  line  PV,.  Since  the  cone  is 
right  circular,  this  sphere  is  tangent  to  each  element  of  the  cone,  and  all  the  points 
of  tangency  are  on  a circle  BjEj.  Further,  DjHj  is  the  line  of  intersection  of  the 
plane  of  the  circle  and  the  plane  n and  is  perpendicular  to  BjDj. 
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Let  M be  any  point  of  the  curve  in  which  the  plane  cuts  the  cone,  and  PLM 
the  element  of  the  cone  through  M,  L being  its  point  of  tangency  to  the  sphere. 
Since  MFX  and  ML  are  tangents  to  the  sphere  from  the  same  outside  point,  they  are 
equal.  Further,  all  the  elements  of  the  cone  make  the  same  angle  with  the  plane 
E^jL.  Therefore,  the  line  ML  makes  with  this  plane  an  angle  equal  to  V^Dj.  It 
follows  that  (Lehmann,  1964) 

MFj  ML  sin  /VABj 

= = = const.  (2.23) 

MN  MN  sin  /VfoD, 

where  N is  the  foot  of  the  perpendicular  from  M on  the  line  DjHj.  Thus  the  curve 
is  a conic,  Fj  being  the  focus  and  the  directrix. 

When  the  angle  VjDjBj  is  less  than  the  angle  VjBjDj,  in  which  case  the  ratio 
in  Eq.  (2.23)  is  less  than  1,  the  plane  intersects  all  the  elements  of  the  cone  and  the 
section  is  an  ellipse.  As  the  angle  V^Bj  is  taken  smaller  and  smaller,  the 
eccentricity  becomes  smaller  and  approaches  the  value  0,  in  which  case  the  plane  is 
normal  to  the  axis  of  the  cone  and  the  plane  section  is  a circle. 

When  the  angle  V^B,  is  equal  to  the  angle  VjB^,  that  is,  when  the  line 
D^jG’  is  parallel  to  the  element  PE^  in  which  case  plane  n is  parallel  to  PE„  the 
ratio  in  Eq.(2.23)  is  equal  to  + 1,  and  the  plane  section  is  a parabola. 

When  the  angle  V^Bj  is  greater  than  the  angle  VjBjDj,  in  which  case  the 
ratio  in  Eq.  (2.23)  is  greater  than  + 1,  the  plane  intersects  only  some  of  the  elements 
of  the  cone  and  the  section  is  one  branch  of  a hyperbola,  the  other  branch  being  the 
section  of  the  cone  obtained  by  extending  the  elements  through  P.  If  we  take  a 
plane  through  P parallel  to  plane  n,  it  intersects  the  two  cones  in  two  elements; 
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when  these  are  projected  orthogonally  upon  plane  n,  the  resulting  lines  are  the 
asymptotes  of  the  hyperbola,  and  their  point  of  intersection,  that  is,  the  projection 
of  P,  is  the  center  of  the  hyperbola. 

Since  the  perspective  projection  preserves  the  incidence  relation,  the 
intersection  of  the  projected  conic  and  the  base  of  the  cone  determines  the  location 
where  elements  of  the  cone  intersect  the  sphere  Sob.  The  projected  conic  is 
symmetric  with  respect  to  the  plane  which  contains  the  axis  of  the  cone  and  the 
center  of  the  sphere  Sob.  Thus,  the  conic  is  symmetric  with  respect  to  the  line 
connecting  its  center  and  the  center  of  the  base  Cb  of  the  cone  (see  Fig.  2.12).  This 
observation  is  important  in  finding  intersection  points  of  the  projected  conic  and  the 
base  of  the  cone. 


2.5  Intersection  of  a Cone  and  a Cylinder 

In  this  section,  the  problems  of  detection  and  determination  of  intersection 
of  a right  circular  cone  and  a cylinder  is  discussed.  In  this  study,  a right  circular 
cone,  as  stated  in  the  previous  section,  is  traced  by  a link  with  a fixed  end  while 
the  other  end  lies  on  a circle.  A cylinder  is  used  to  model  a robot  link  in  later 
application. 

As  shown  in  Fig.  2.13,  let  r2  be  a plane  which  passes  through  the  axis  of  a 
cylinder  and  being  parallel  to  the  axis  of  a cone  and  be  a plane  which  passes 
through  the  axis  of  the  cone  and  is  perpendicular  to  r2.  Using  the  plane  sections  of 
the  cylinder  and  the  cone  in  r„  it  can  be  shown  that  if  the  distance  of  the  center  of 
the  plane  section  (an  ellipse)  of  the  cylinder  to  a line  passing  through  one  foot  and 
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perpendicular  to  the  base  of  the  plane  section  (an  isosceles  triangle)  of  the  cone  is 
less  than  the  radius  of  the  cylinder,  the  cylinder  and  the  cone  will  not  intersect. 
Further,  if  the  plane  section  of  the  cylinder  in  r2  does  not  intersect  with  the 
perspective  projection  of  the  cone  from  a point  at  infinity  onto  r2,  the  cylinder  and 
the  cone  will  not  intersect. 

If  the  possibility  of  intersection  between  the  cone  and  the  cylinder  is  positive, 
it  is  necessary  to  determine  the  locations  of  intersection.  This  can  be  done  by  first 
projecting  the  cylinder  from  the  vertex  of  the  cone  onto  the  base  plane  of  the  cone. 
Then,  finding  the  intersection  of  the  boundary  of  the  projection  figure  and  the  base 
of  the  cone. 

The  perspective  projection  of  a cylinder  from  the  vertex  to  the  base  plane 
of  a cone  can  be  performed  in  three  steps.  First,  the  section  of  the  cylinder  between 
the  base  plane  of  the  cone  and  a parallel  plane  passing  the  vertex  of  the  cone  is 
identified.  Second,  the  two  conics  on  both  ends  of  the  identified  cylinder  are 
projected.  This  results  in  two  conics  on  the  projection  planes.  Finally,  the  trunk  of 
the  cylinder  is  projected.  This  results  in  two  line  segments  on  the  projection  plane, 
and  they  are  tangent  to  the  above  two  conics  (see  Fig.  2.14). 

In  order  to  find  the  projection  of  a conic,  a transformation  matrix  which  is 
associated  with  this  projection  is  determined.  A projection  transformation  of  a plane 
onto  the  other  plane  is  completely  determined  by  four  distinct  pairs  of  corresponding 
points,  no  three  are  colinear.  Let  the  projection  transformation  be  represented  by 
a 3x3  matrix  [M]  which  transforms  a point  of  a plane  n in  homogeneous  coordinates 
onto  a point  in  the  projection  plane  n,  thus 
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CTXj 

*1 

ctx2 

= [M] 

x2 

ctx3 

1 

X 

1 

and  which  has  the  inverse, 


(2.24) 


MXx 

xi 

MX2 

= [M]1 

X2 

MX3 

X3 

A conic  f(X)  in  the  plane  tt  of  the  form, 


(2.25) 


f(X)  = ajjXjXj  = 0 (2.26) 

is  mapped  into  a conic  g(x)  in  n under  this  transformation  and 

g(x)  = bijXjXj  = 0 (2.27) 

where  by  can  be  determined  by  replacing  xs  and  Xj-  in  Eq.  (2.26)  by  Eq.  (2.25). 

The  projection  of  the  trunk  of  the  cylinder  is  bounded  by  two  line  segments. 
In  fact,  these  two  line  segments  lie  on  the  intersections  of  the  projection  plane  and 
two  planes  passing  through  the  center  of  the  projection  and  being  tangent  to  the 
cylinder.  Note  that  if  the  cylinder  intersects  with  the  plane  passing  through  the 
vertex  of  the  cone  and  being  parallel  to  the  base  plane  of  the  cone,  these  two  line 
segments  have  endpoints  which  are  points  at  infinity. 

By  computing  the  intersection  of  these  two  projection  conics  and  line 
segments  with  the  base  of  the  cone,  the  locations  of  the  intersection  between  the 
cone  and  the  cylinder  are  determined. 
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Fig.  2.1  Transformation  of  relative  location  of  a pair  of  line  segments. 


Fig.  2.2  possible  cases  for  the  image  point  being  in  the  boundary  of 
nonallowable  manifold  (NM). 
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Fig.  2.4  Region  of  intersection. 
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Fig.  2.5  possible  cases  for  the  relative  position  being  on  the  boundary 
of  region  of  intersection  (RI). 
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Fig.  2.6  Transform  an  ellipse  into  a circle. 


Fig.  2.7  Transformation  of  parallel  lines. 
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Fig.  2.8  A right  circular  cone  and  a sphere. 
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Fig.  2.9  Plane  sections  of  cone  and  spheres. 


Fig.  2.10  Polar  plane  of  P with  respect  to  Sob. 


27 


Fig.  2.11  Plane  section  of  a cone. 


Fig.  2.12  Projection  conic. 
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Fig.  2.13  A cone  and  a cylinder. 

P 


Fig.  2.14  Perspective  projection  of  a cylinder. 


CHAPTER  3 

INTERFERENCE  AVOIDANCE  OF 
LINE-SEGMENT-MODELED  ROBOT  MANIPULATORS 


This  chapter  is  devoted  to  the  development  of  algorithms  which  are  applied 
to  a robot  manipulator  to  avoid  obstacles  or  a second  robot  manipulator  in  the 
workspace  while  following  a prespecified  path.  The  robot  manipulators  are  modeled 
by  a sequence  of  line  segments  connected  by  joints. 

TRS  robots  and  PUMA  robots  are  considered  in  this  study.  The  workspace 
of  a TRS  robot  is  bounded  by  two  concentric  spheres  (Lovell,  1983).  Due  to  the 
presence  of  the  second  joint  offset,  the  workspace  of  the  PUMA  robot  has  a 
cylindrical  inner  boundary.  The  outer  boundary  is,  however,  a sphere 
(Krishnamurthy,  1985). 

Permissible  orientations  which  describe  the  geometric  capability  of  a robot 
manipulator  when  its  end  effector  is  pinned  at  a point  are  discussed  in  Section  3.1. 
Then,  the  determination  of  allowable  orientations  of  robot  links  in  which  they  avoid 
interference  with  spherical  and  polyhedral  obstacles  is  demonstrated  in  Section  3.2. 
In  Section  3.3,  a procedure  for  determining  favorable  orientations  of  the  robot  links 
is  presented.  It  should  be  noted  that  the  derivation  of  the  allowable  orientations  and 
the  favorable  orientations  of  robot  links  are  performed  on  a plane  of  operation 
which  is  the  plane  where  the  forearm  of  the  robot  lies.  Finally,  Section  3.4  discusses 
two  types  of  operations  that  can  be  used  for  the  coordination  of  two  robot 


29 


30 


manipulators  working  in  a common  workspace.  The  first  type  of  operations  is 
referred  to  as  master-slave  operations,  and  the  second  type  of  operations  is  referred 
to  as  parallel  operations.  Due  to  the  complexity  of  the  problem,  the  robots  are 
restricted  to  rotary  planar  robots  in  the  parallel  operations. 

In  subsequent  discussion,  the  analysis  are  mainly  for  the  TRS  robots  unless 
otherwise  specified.  However,  the  analysis  can  be  extended  to  the  PUMA  robots 
and  can  be  found  in  separate  sections. 

3.1  Permissible  Orientations  of  Robot  Links 
3.1.1  Spatial  Consideration 

End  effector.  In  order  to  determine  the  permissible  orientations  of  the  end 
effector  of  a TRS  robot  (Fig.  3.1)  or  a PUMA  robot  (Fig.  3.2),  a reference 
coordinate  system  Oxyz  can  be  selected  such  that  its  origin  coincides  with  the 
intersection  point  of  the  first  two  joints  of  the  robot,  i.e.,  the  shoulder  of  the  robot. 
It  is  assumed  that  a path  of  the  end  effector  has  been  specified.  For  a target  point 
P(xp,  yp,  zp)  in  the  specified  path,  the  end  effector  can  be  considered  connected  with 
the  point  P by  a fixed  hypothetical  spherical  joint.  The  wrist  joint  of  the  robot  is 
thus  free  to  move  on  a spherical  surface  which  will  be  called  constraint  surface  and 
can  be  determined  by  the  intersection  of  the  concentric  spheres  SP,  i and  SP,  o 
centered  at  O and  the  sphere  SP,  centered  at  P (see  Fig.  3.3).  For  TRS  robots  (see 
Fig.  3.1),  the  radii  of  SP„  SP,_i  and  SP,_o  are  r,  = S66,  r3i  = |a23-Sj  and  r3o  = 
&23  + S44,  respectively.  For  PUMA  robots  (see  Fig.  3.2),  r3  = S*,  r3i  = [(a23-SJ2  + 

( S22~S33)2]  */2  and  r^  = [(a^  + S^)2  + (S22-S33)'] 1/". 
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For  convenience,  a pair  of  transformations  of  coordinate  system  will  be 
introduced  (see  Fig.  3.4).  First  the  Ox’y’z’  system  is  obtained  by  rotating  the  Oxyz 
system  about  the  z axis  through  an  angle  a.  Next  the  Ox"y"z"  system  is  derived  by 
rotating  the  Ox’y’z’  system  about  the  y’  axis  through  an  angle  (n/2-p),  where  a and 
p are  given  by 

a = tan_1(yp  / xp)  (3.1) 

and 

p = tan-1(zp  / (Xp2  + yp2)1/2)  (3.2) 

Using  the  Ox"y"z"  coordinate  system,  the  coordinates  of  the  target  point  P 
become  (0,0, d),  where  d is  the  distance  of  the  target  point  P to  the  origin  O,  i.e., 
d = (V  + yp2  + zp2)1/2  (3.3) 

The  equations  of  the  spheres  SP3,  SP3_i  and  SP,_o  can  be  expressed  in  Oxyz  system 
as  follows 


SP3: 

x2  + y2  + (z  - d)2  = r32 

(3.4) 

SP3_i: 

x2  + y2  + z2  = r3i2 

(3.5) 

SP3_o 

: x2  + y2  + z2  = rj 

(3.6) 

The  intersection  of  SP3  and  SP3_o  is  a circle  (assuming  they  intersect),  the  equation 
of  which  in  parametric  form  is 

X"  = S^COSijCOSU 

y"  = S66cos53sinu  0 < u < 2n  (3.7) 

z"  = d + S66sin<S3 
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where  <S3  and  u are  latitude  and  longitude  of  the  sphere  SP3.  Analogously,  the  circle 
determined  by  the  intersection  of  SP3  and  SP3_i  can  be  expressed  in  the  same  form 
by  replacing  <S3  with  S4  (see  Fig.  3.5). 

The  permissible  orientation  of  the  end  effector  can  be  expressed  in  terms  of 
the  latitudes  and  longitudes  of  the  sphere  SP3.  Any  point  on  the  constraint  surface 
can  thus  be  represented  in  the  Ox"y"z"  coordinate  system  by 
x"  = S66cosvcosu 

y"  = S^cosvsinu  (3.8) 

z"  = d + S66sinv 

where  <S4  < v < <53  and  0 < u < 27t.  Note  that  when  SP3  and  SP3_o  do  not  intersect 
with  each  other,  63  = n/2  and  when  SP3  and  SP3_i  do  not  intersect  with  each  other, 
<5 4 = -7t/2. 

Upperarm  and  forearm.  To  determine  the  permissible  orientations  of  the 
upperarm  and  the  forearm,  it  is  helpful  to  consider  these  two  links  together.  Since 
the  joint  connecting  the  upperarm  and  the  forearm  of  a TRS  robot  is  a revolute 
joint,  these  two  links  will  always  lie  on  a plane  passing  through  the  z axis.  In 
addition,  this  plane  belongs  to  a pencil  of  planes  with  the  z axis  as  the  axis  of  the 
pencil  (see  Fig.  3.6).  Two  planes  of  this  pencil  which  are  tangent  to  the  sphere  SP3 
determine  the  boundary  of  this  pencil  that  the  upperarm  and  the  forearm  can  lie  on. 
For  convenience,  these  planes  will  be  called  planes  of  operation  or  n planes 
hereafter.  In  the  work  of  Choi  (1990)  they  are  called  the  profile  planes. 
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It  is  observed  that  each  n plane  intersects  the  sphere  SP,  in  a circle,  with  the 
exception  of  the  pair  of  tangent  planes  which  have  a point  contact  with  the  sphere. 
The  permissible  orientation  ranges  of  the  upperarm  and  the  forearm  on  a it  plane 
depend  upon  where  this  it  plane  is  located.  In  each  n plane,  the  computation  of  the 
permissible  orientations  can  be  performed  by  considering  the  robot  to  be  a planar 
3R  robot  with  three  links.  The  lengths  of  the  first  two  links  are  the  same  as  those 
of  the  upperarm  and  the  forearm  of  the  TRS  robot.  Whereas,  the  length  of  the 
third  link  (the  end  effector)  of  the  planar  robot  is  equal  to  the  radius  of  the  circle 
of  intersection  of  the  it  plane  and  the  sphere  SP3.  In  addition,  the  center  of  the 
circle  is  the  position  where  the  reference  point  of  the  end  effector  of  the  planar 
robot  is  located  (see  Fig.  3.7). 

Thus  for  each  n plane,  the  permissible  orientation  ranges  of  the  upperarm 
and  the  forearm  can  be  computed  by  analyzing  the  motion  capability  of  a planar  3R 
robot  or  a planar  four-bar  linkage  which  is  presented  in  the  next  section.  The 
permissible  orientations  of  the  upperarm  and  the  forearm  of  the  TRS  robot  can  thus 
be  determined  by  the  union  of  the  permissible  orientations  of  these  two  links  on 
each  7 t plane. 

3.1.2  Planar  Consideration 

When  the  position  of  a reference  point  on  the  end  effector  of  a planar  3R 
robot  is  fixed,  the  mechanism  of  the  planar  robot  becomes  a four-bar  linkage.  The 
location  of  the  wrist  joint  of  the  robot  is  restricted  on  a constraint  curve  which  can 
be  determined  by  the  intersection  of  the  concentric  circle  C3_i  and  C,_o  centered  at 
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the  reference  point  P’  and  the  circle  C3  centered  at  shoulder  O (see  Fig.  3.8).  In 
fact,  this  constraint  curve  is  the  intersection  of  the  n plane  where  the  robot  links  lie 
and  the  constraint  surface  described  in  Section  3.1.1.  The  constraint  curve  for  the 
elbow  joint  can  be  derived  analogously  (see  Fig.  3.9). 

The  permissible  orientations  of  links  for  a planar  four-bar  mechanism  are 
dependent  upon  its  link  lengths.  A four-bar  mechanism  may  be  a so-called  crank- 
rocker,  double-crank  or  double-rocker  linkage,  depending  on  the  range  of  motion  of 
the  two  links  connected  to  the  ground  link. 

There  are  four  possible  types  for  the  range  of  permissible  end  effector 
orientations  according  to  whether  the  constraint  circle  C,  intersects  the  circles  C3_i 
and  C3_o.  In  other  words,  they  depend  upon  the  relationship  between  d + S66  and  the 
radius  of  C3_o,  a23  + S^,  together  with  that  between  d-S66  and  the  radius  of  C3_i,  | a23- 
S44 1 , where  d,  a23,  S^,  and  are  link  lengths.  These  conditions  are  illustrated  in 
Fig.  3.10(a)  - (d).  Similarly,  four  types  can  be  obtained  for  the  range  of  permissible 
upperarm  orientations.  However,  for  a given  set  of  link  lengths,  the  number  of  types 
is  reduced.  For  instance,  there  are  only  three  types  for  a robot  with  link  lengths 
satisfying  the  conditions  S44  > a23  > S66.  Each  type  is  dependent  upon  the  distance 
of  the  target  point  to  the  shoulder,  viz.,  d > a23  + S44-S66,  -a23  + S44+S66  < d < a^+S^- 
S66  and  d < -a23  + + S66.  For  each  type,  the  configurations  of  the  robot,  i.e.,  elbow 

up  (down)  and  wrist  up  (down),  must  be  taken  into  account  in  order  to  determine 
the  ranges  of  the  permissible  orientations  of  the  upperarm  and  the  end  effector. 

In  conclusion,  based  upon  the  position  of  the  target  point  and  the  desired 
configuration  of  the  robot  manipulator,  ranges  of  permissible  orientations  for  the 
upperarm  and  the  end  effector  can  be  computed. 
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3.1.3  Permissible  Orientations  of  PUMA  Robots 

Since  there  is  an  offset  along  the  elbow  joint,  the  upperarm  and  the  forearm 
of  a PUMA  robot  do  not  lie  on  the  same  plane.  However,  the  upperarm  and  the 
forearm  lie  on  a pair  of  parallel  vertical  planes  separated  by  a fixed  distance  S33,  the 
offset  of  the  elbow.  Therefore,  a pencil  of  pairs  of  vertical  planes  can  be  selected 
such  that  the  upperarm  lies  on  one,  which  is  denoted  by  na,  of  the  parallel  planes 
and  the  forearm  lies  on  the  other,  which  is  denoted  by  nb,  as  shown  in  Fig.  3.11. 
The  boundary  of  all  possible  plane  pairs  (na  and  7rb)  is  determined  by  the  two  nb 
planes  which  are  tangent  to  the  sphere  SP3.  The  upperarm  and  the  forearm  can  lie 
on  any  plane  pair  between  the  boundary  plane  pairs. 

In  order  to  compute  the  permissible  orientations  of  the  upperarm  and  the 
forearm  for  a pair  of  specific  na  and  7rb  planes,  it  is  necessary  to  introduce  a 
hypothetical  link  a23’  which  is  a perspective  projection  of  the  upperarm  onto  the  irb 
plane  from  a center  which  is  a point  at  infinity  on  a line  normal  to  the  planes  ira  and 
nb.  The  resulting  mechanism  in  the  plane  7rb  is  a four-bar  linkage,  and  the 
development  for  the  TRS  robots  in  the  previous  sections  can  be  applied  to  the 
PUMA  robots. 


3.2  Allowable  Orientations  of  Robot  Links 


The  allowable  orientations  of  a link  are  the  orientations  where  the  link  does 
not  collide  with  obstacles  in  the  workspace.  In  this  section,  the  upperarm  and  the 
forearm  of  a robot  manipulator  are  assumed  to  be  on  a n plane.  The  computation 
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of  allowable  orientations  of  each  link  such  that  it  avoids  spherical  and  polyhedral 
obstacles  is  demonstrated. 

3.2.1  Polyhedral  Obstacles 

If  the  7r  plane  where  the  upperarm  and  the  forearm  lie  intersects  with  an 
obstacle  which  is  modeled  by  a convex  polyhedron,  the  intersection  is,  in  general,  a 
polygon.  Since  the  position  of  the  shoulder  of  the  manipulator  is  fixed,  the 
upperarm  sweeps  a circular  region  in  the  n plane.  The  allowable  orientations  of  the 
upperarm  are  then  determined  by  a circular  section  with  center  O and  radius  a23 
and  which  is  outside  the  polygon. 

The  allowable  orientations  of  the  forearm  are  the  orientations  where  the  link 
does  not  intersect  with  the  polygon.  In  order  to  find  the  allowable  orientations,  each 
side  of  the  polygon  needs  to  be  considered  in  sequence.  For  each  side  of  the 
polygon,  a range  of  orientations  within  which  the  forearm  does  not  intersect  this  side 
is  determined  by  using  the  transformation  for  a pair  of  line  segments.  The 
intersection  of  these  ranges  of  orientations  for  all  sides  of  the  polygon  gives  the 
allowable  orientations  of  the  forearm.  However,  if  the  endpoints  of  the  forearm  do 
not  intersect  with  the  polygon,  in  other  words,  the  forearm  is  in  a range  of 
orientations  where  the  upperarm  and  the  end  effector  do  not  intersect  the  polygon, 
the  forearm  needs  only  to  avoid  all  vertices  of  the  polygon.  This  statement  can  be 
perceived  by  the  description  of  possible  boundary  crossings  in  Section  2.1. 

Refer  to  Fig.  3.12,  the  relation  between  the  orientations  of  the  forearm  and 
the  upperarm  can  be  expressed  in  the  form  (Hunt  1978) 
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R1cos$2  + R2cos$3  - cos($2  - $3)  + R3  = 0 (3.9) 

where 

Ri  = d / s.4 

R2  = d / a23 

^■3  = (S66  ‘ a23  ‘ S44  ' d ) / 2a23S44 

a23,  S44,  and  s66  are  link  lengths,  d is  the  distance  of  the  fixed  reference  point  on  the 
end  effector  to  the  shoulder  O,  and  $2>  $3  are  orientations  of  the  upperarm  and  the 
forearm,  respectively.  For  each  $2,  two  values  of  $3  can  be  computed  which 
correspond  to  the  wrist  up  and  the  wrist  down  configurations. 

In  order  to  check  when  the  forearm  intersects  a line  segment  GH  in  the  same 
plane,  the  transformation  presented  in  Section  2.1  can  be  applied.  Suppose  that  the 
upperarm  a23  rotates  within  an  angular  range,  the  relative  locations  of  the  forearm 
S44  and  the  line  segment  GH  can  be  transformed  into  a curve  in  a projection  plane. 
The  section  of  this  curve  outside  the  Nonallowable  Manifold  (NM)  gives  a range  of 
orientations  of  the  forearm  within  which  the  forearm  does  not  intersect  the  line 
segment  GH.  In  Fig.  3.13,  an  illustration  of  image  curve  of  relative  location  of  the 
forearm  with  respect  to  a line  segment  is  presented. 

In  order  to  find  the  image  curve  outside  (NM),  it  is  necessary  to  find  the 
intersection  point(s)  of  the  image  curve  with  the  boundary  of  (NM).  It  is  noted  that 
if  the  upperarm  and  the  end  effector  do  not  intersect  line  segment  GH,  the  image 
curve  of  the  relative  locations  of  the  forearm  and  the  line  segment  will  not  cross  the 
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boundary  of  (NM)  via  cases  (b)  and  (d)  as  described  in  Section  2.1.  Therefore, 
only  cases  (a)  and  (c)  are  of  interest. 

When  the  image  point  is  in  the  case  (a),  the  following  equation: 

X = 0 (3.10) 

together  with  Eq.  3.9  must  be  satisfied  simultaneously. 

Replacing  X by  Eq.  2.6,  Eq.  3.10  becomes 

x - ycotfq  = 0 (3.11) 

or 

(a23cos§2  - xG)  - (a23sin$2  - yG)cot$3  = 0 (3.12) 

where  (xG,  yG)  are  coordinates  of  the  endpoint  G of  the  line  segment  GH.  Here, 
the  line  segment  GH  is  assumed  to  be  parallel  to  the  x axis  of  the  reference 
coordinate  system  (see  Fig.  3.12). 

Equations  3.9  and  3.12  are  both  nonlinear  with  unknowns  $2  and  $3.  In  order 
to  solve  a set  of  nonlinear  equations,  Newton’s  method  (Conte  and  de  Boor  1980) 
which  has  a quadratic  convergence  is  used. 

In  order  to  find  the  allowable  orientations  of  the  end  effector,  it  is  necessary 
to  consider  the  intersection  of  a circular  cone  and  the  polyhedral  obstacle.  For  each 
face  of  the  polyhedron,  a perspective  projection  with  center  at  the  vertex  of  the  cone 
can  be  used  to  project  this  face  onto  the  base  plane  of  the  cone  (see  Fig.  3.14).  The 
intersection  of  the  projected  polygon  with  the  base  of  the  cone  can  be  used  to 
determine  the  orientations  of  the  end  effector  outside  this  face.  The  intersection  of 
orientations  of  the  end  effector  that  do  not  collide  with  each  face  of  the  polyhedron 
gives  the  allowable  orientations  of  the  end  effector. 
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It  is  necessary  to  identify  the  location  of  faces  of  a polyhedron  before 
applying  the  projection.  Let  r be  a plane  passing  through  the  vertex  of  the  cone  and 
parallel  to  the  plane  n,  the  base  plane  of  the  cone.  Since  intersection  of  a face  and 
the  cone  may  occur  only  at  the  section  of  the  face  that  lies  between  the  planes  n and 
r,  only  this  section  of  the  face  is  of  interest.  As  shown  in  Fig.  3.15,  faces  and 
F2  do  not  lie  between  the  planes  r and  n,  thus  these  two  faces  will  not  intersect  the 
cone  and  the  projection  is  not  necessary  for  them.  In  contrast,  faces  F3  and  F4 
intersect  with  r or  n,  and  the  projection  is  applied  only  to  the  sections  of  the  faces 
which  lie  between  the  planes  r and  tt. 

3.2.2  Spherical  Obstacles 

In  general,  the  intersection  of  a n plane  and  a spherical  obstacle  Sob 
(assuming  they  intersect)  is  a circle  which  will  be  denoted  by  Cob  (Fig.  3.16). 
Therefore,  the  allowable  orientation  range  of  the  upperarm  is  determined  by  a 
circular  section  with  center  O (shoulder)  and  radius  a23  (upperarm  length)  which  is 
outside  the  circle  Cob. 

The  endpoints  of  the  forearm  are  constrained  by  the  elbow  and  the  wrist 
joints  which  are  moving  along  circular  arcs.  In  order  to  find  the  allowable 
orientations  where  the  forearm  avoids  the  circle  Cob,  the  transformation  for  the 
relative  position  of  a line  segment  and  a circle  as  described  in  Section  2.2  can  be 
used.  In  Fig.  3.17,  an  illustration  of  image  curve  of  relative  positions  of  the  forearm 
with  respect  to  a circle  is  presented.  In  order  to  find  the  image  curve  outside  (RI), 
the  numerical  procedure  presented  in  the  previous  section  is  used. 
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When  the  upperarm  and  the  forearm  lie  on  a ir  plane,  the  end  effector  lies 
on  a right  circular  cone.  The  vertex  of  this  cone  is  the  target  point  P and  the  base 
Cb  of  this  cone  lies  on  the  ir  plane.  The  detection  of  intersection  between  a right 
circular  cone  and  a sphere  can  then  be  accomplished  by  using  the  perspective 
projection  described  in  Section  2.4. 

3.3  Favorable  Orientations 

In  the  previous  section,  the  ranges  of  allowable  orientation  of  three  links  of 
the  robot  have  been  computed.  In  order  to  determine  the  favorable  orientations  of 
a robot  with  respect  to  a specific  ir  plane,  the  following  procedure  will  be  used: 

(a)  Determine  the  orientations  of  the  upperarm  corresponding  to  the  allowable 
orientations  of  the  end  effector. 

(b)  Determine  the  intersection  of  the  allowable  orientations  of  the  upperarm  and 
the  orientations  found  in  (a). 

(c)  Determine  the  orientations  of  the  upperarm  in  the  region  obtained  in  (b) 
such  that  the  forearm  is  in  its  allowable  orientation  range. 

This  procedure  yields  an  orientation  range  of  the  upperarm  within  which  all  links  of 
the  robot  do  not  intersect  the  obstacles. 

Finally,  it  is  necessary  to  emphasize  that  the  robot  manipulator  is  to  follow 
a prespecified  path.  At  any  instant,  a target  point  is  specified  and  a series  of  ir 
planes  is  obtained  based  upon  the  location  of  the  upperarm  and  the  forearm  at  the 
previous  instant  and  the  velocity  constraint  on  the  base  joint  of  the  robot.  For  each 
7r  plane,  a range  of  favorable  orientations  can  be  obtained.  The  location  of  the  ir 
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plane  where  the  robot  has  a local  maximum  favorable  orientation  range  can  be 
identified  numerically,  and  the  upperarm  and  the  forearm  of  the  robot  is  relocated 
at  this  7r  plane.  The  robot  can  have  joint  angles  within  the  favorable  orientations 
and  is  guaranteed  to  be  free  from  collision. 

3.4  Coordination  of  Two  Robot  Manipulators 

Due  to  high  complexity  in  the  computation,  two  simplified  operations  are 
considered  here,  and  strategies  are  proposed  which  are  applied  to  the  two  types  of 
operations. 

The  first  type  of  operations  is  referred  to  as  master-slave  operations.  In  this 
type  of  operations,  one  robot  manipulator  is  assigned  as  the  master  and  the  motion 
of  which  has  precedence  over  that  of  the  second  robot  manipulator  (the  slave).  The 
slave  robot  must  adapt  its  motion  to  the  master  robot. 

The  second  type  of  operations  is  referred  to  as  parallel  operations.  In  this 
type  of  operations,  both  robot  manipulators  possess  the  same  status,  and  they  must 
accommodate  to  each  other.  Further,  the  manipulators  are  modeled  by  rotary  planar 
manipulators.  By  a rotary  planar  manipulator  we  mean  a manipulator  whose  links 
with  revolute  joints  lie  in  a plane  which  can  rotate  around  the  vertical  axis  of  the 
robot’s  base.  It  is  assumed  that  the  links  of  a robot  manipulator  always  lie  on  the 
same  plane  during  the  motion. 

3.4.1  Master-Slave  Operations 

It  is  assumed  that  the  motion  of  the  master  robot  has  been  completely 
specified.  In  other  words,  the  locations  of  links  of  the  master  robot  are  known  at 
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any  time  during  the  motion.  The  slave  robot  is  to  avoid  interference  with  the  links 
of  the  master  robot  while  tracking  a specified  trajectory. 

Following  the  analysis  in  Section  3.1,  when  the  upperarm  and  the  forearm  of 
the  slave  robot  lie  on  a n plane,  this  plane  will  intersect  the  links  of  the  master 
robot  in  points  unless  the  links  of  the  master  robot  lie  on  this  plane.  Therefore,  in 
order  to  avoid  interference  with  the  master  robot,  the  upperarm  and  the  forearm  of 
the  slave  robot  must  avoid  intersecting  these  points. 

The  area  swept  by  the  upperarm  is  a circular  region  with  radius  equal  to  its 
link  length.  If  a point  lies  within  this  circular  region,  the  upperarm  may  intersect 
this  point.  To  compute  an  allowable  orientation  range,  it  is  necessary  to  find  the 
orientation  where  the  upperarm  intersects  the  point.  This  orientation  will  divide  the 
permissible  orientations  into  two  regions.  Based  upon  the  orientation  of  the 
upperarm  at  the  previous  time  instant,  one  of  the  permissible  orientation  regions  is 
selected  as  the  allowable  orientation  region. 

The  end  points  of  the  forearm  are  constrained  by  the  elbow  joint  and  the 
wrist  joint  which  are  moving  along  circular  arcs.  Therefore,  the  forearm’s 
orientation,  as  well  as  the  positions  of  its  end  points,  varies  constantly.  In  order  to 
determine  where  the  forearm  intersects  a point,  the  transformation  for  relative 
locations  of  a line  segment  and  a point  can  be  used. 

After  finding  the  orientations  where  the  forearm  intersects  a point,  a range 
of  orientations  can  be  obtained  which  is  consistent  with  the  orientation  of  the 
forearm  at  the  previous  time  instant  to  avoid  interference. 
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As  described  in  the  previous  section,  the  end  effector  encloses  a right  circular 
cone  when  the  upperarm  and  the  forearm  lie  on  a specific  n plane.  In  order  to  find 
the  location  where  the  end  effector  intersects  a link  of  the  master  robot,  a 
perspective  projection  can  be  applied.  This  projection  projects  each  link  of  the 
master  robot  from  the  vertex  of  the  cone  into  a line  segment  in  the  base  plane  of 
the  cone.  Since  the  end  effector  lies  on  one  side  of  the  base  plane,  only  the  section 
of  the  line  segment  that  lies  on  the  same  side  as  the  end  effector  is  of  interest.  The 
intersection  point(s)  of  the  projected  line  segment  and  the  base  of  the  cone  can  be 
used  to  find  the  location  where  the  end  effector  intersects  the  links  of  the  master 
robot.  Finally,  the  ir  plane  where  the  robot  manipulator  has  the  largest  favorable 
orientation  range  is  computed  numerically. 

3.4.2  Parallel  Operations 

In  this  section,  two  robot  manipulators  are  considered  having  the  same  status. 
To  perform  the  operation,  each  robot  manipulator  has  been  assigned  a trajectory 
regardless  of  orientation.  One  robot  manipulator  has  to  avoid  interference  with  the 
other  based  upon  the  information  of  target  positions  and  motion  capability  of  links 
of  both  robot  manipulators. 

At  any  instant,  in  order  to  obtain  a configuration  for  each  robot  manipulator 
to  avoid  interference,  it  is  necessary  to  determine  first  a specific  tt  plane  where  the 
upperarm  and  the  forearm  lie  and  then  a set  of  joint  angles  that  the  manipulator 
holds.  This  will  unavoidably  bring  significant  complexity  into  analysis.  In  order  to 
simplify  the  analysis,  it  is  convenient  to  place  the  upperarm  and  the  forearm  of  the 
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robot  manipulator  on  a specific  n plane,  in  particular,  the  plane  which  passes 
through  the  specified  target  point  of  the  end  effector.  In  this  perspective,  all  three 
links  of  the  robot  manipulator  will  lie  on  this  it  plane.  This  implies  that  2 degrees 
of  freedom  (DOF)  in  the  wrist  joint  will  become  inactive  and  the  robot  manipulator 
becomes  4 DOF.  Therefore,  the  interference  avoidance  problem  between  two  TRS 
robots  is  reduced  to  that  between  two  planar  3R  robot  manipulators  on  different 
planes  in  space. 

Forbidden  regions.  When  two  robot  manipulators  lie  on  two  different  planes 
in  the  space,  the  only  possible  location  for  interference  is  at  the  intersection  line  of 
these  two  planes.  As  shown  in  Fig.  3.18,  one  TRS  robot  lies  on  the  plane  7rl5  and 
the  other  TRS  robot  lies  on  the  plane  n2.  The  line  / is  the  intersection  of  the  planes 
nx  and  ir2.  The  points  P:  and  P2  are  target  points  for  the  end  effectors  of  two  robot 
manipulators. 

As  the  links  of  one  robot  manipulator  move  within  the  permissible  orientation 
range,  the  links  will  intersect  / in  points  which  form  one  or  two  line  segments.  In 
the  following  analysis,  only  one  line  segment  case  is  considered.  The  two  line 
segments  case  can  be  analyzed  similarly. 

In  order  to  find  the  endpoints  of  the  line  segment,  it  is  convenient  to  discuss 
the  intersection  of  / and  each  link  of  the  robot  manipulator  separately.  The  union 
of  the  line  segments  determined  from  each  link  yields  the  positions  where  the  robot 
links  may  intersect  /. 

The  endpoints  of  a line  segment  generated  by  the  intersection  of  / and  the 
upperarm  or  the  end  effector  can  be  obtained  by  considering  the  intersection  of  / 
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and  a circular  sector.  The  endpoints  of  a line  segment  generated  by  the  intersection 
of  / and  the  forearm  cannot  be  derived  explicitly.  Thus,  an  approximation  for  the 
endpoints  will  be  obtained  by  the  aid  of  intersection  points  of  / and  tangents  from 
the  circle  C2  to  the  circle  C3.  In  Fig.  3.19,  the  approximated  boundaries  for  the 
locations  of  the  links  of  a robot  manipulator  are  illustrated,  where  the  robot 
manipulator  has  link  lengths  a23  = 44,  S44  = 55,  and  = 15  units. 

At  the  elbow  up  and  wrist  up  configuration,  the  upper  bound  of  the  forearm 
is  approximated  by  a tangent  from  the  elbow  A2  to  the  circle  C3.  At  the  elbow  up 
and  wrist  down  configuration,  the  lower  bound  of  the  forearm  is  approximated  by 
a tangent  from  Ai  or  A2  to  the  circle  C3.  The  object  of  making  an  approximation 
is  to  confine  the  forearm  by  lines  through  extreme  positions  of  the  elbow  joint  or  the 
wrist  joint  and  tangent  points  on  C2  and  C3.  A comparison  of  the  exact  bound  and 
the  approximated  bound  of  a region  swept  by  the  links  of  a robot  manipulator  is 
shown  in  Fig.  3.20. 

The  intersection  of  the  bounded  region  and  the  line  l represents  the  region 
where  the  links  of  a robot  manipulator  may  traverse  /.  Thus,  two  robot  manipulators 
determine  two  segments  of  / along  which  they  are  able  to  traverse.  If  these  two 
segments  intersect,  the  links  of  the  two  robot  manipulators  may  interfere.  In  order 
to  avoid  interference,  it  is  proposed  to  divide  the  overlapping  region  into  two 
segments  of  same  length.  One  robot  manipulator  must  avoid  moving  along  one 
segment  while  the  other  robot  manipulator  avoids  moving  along  the  other  segment. 
The  segment  within  which  a robot  manipulator  cannot  have  motion  is  called  the 
forbidden  region  of  that  robot  manipulator.  Using  this  concept,  the  allowable 
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orientations  of  a robot  link  can  be  defined  as  the  orientations  in  which  the  link  does 
not  intersect  the  forbidden  region. 

Allowable  orientations.  The  upperarm  is  connected  to  the  shoulder  which  is 
fixed  whilst  the  end  effector  is  instantaneously  joined  to  a fixed  target  point.  Thus 
the  area  swept  by  the  upperarm  and  the  end  effector  are  circular  regions  with  radii 
equal  to  the  link  lengths.  When  the  forbidden  region  lies  within  the  circular  region, 
the  link  may  intersect  the  forbidden  region.  In  this  situation,  the  allowable 
orientation  range  is  determined  either  by  the  intersection  points  of  the  forbidden 
region  and  the  boundary  of  the  circular  region  or  by  the  endpoints  of  the  forbidden 
region. 

The  endpoints  of  the  forearm  are  constrained  by  the  elbow  joint  and  the  wrist 
joint  which  are  moving  along  circular  arcs.  In  order  to  find  the  allowable 
orientations  of  the  forearm,  the  affine  transformation  introduced  in  Section  2.1  can 
be  applied. 

After  finding  the  allowable  orientations,  the  procedure  presented  in  Section 
3.3  can  be  used  to  find  the  favorable  orientations  for  each  robot  manipulator.  The 
robot  links  can  thus  flex  within  their  favorable  orientation  range  and  avoid 
interference  with  the  links  of  the  other  robot  manipulator. 


3.4.3  PUMA  Robots 

Analogous  to  the  TRS  robots  case,  at  any  instant  a specific  pair  of  na  and  nb 
planes  can  be  chosen  such  that  the  complicated  3-D  problem  is  reduced  to  an  easier 


47 


2-D  problem.  In  order  to  achieve  this,  the  specific  plane  pair  is  selected  as  the  one 
whose  7rb  plane  containing  the  target  point  of  the  end  effector.  Thus,  both  the 
forearm  and  the  end  effector  lie  on  the  7rb  plane. 

For  the  master-slave  operation,  the  master  robot  intersects  the  specific  plane 
pairs  of  the  slave  robot  in  points.  The  allowable  orientations  of  a link  of  the  slave 
robot  are  the  orientations  where  the  link  avoids  intersecting  these  points. 

For  the  parallel  operation,  in  order  to  compute  a range  of  allowable 
orientations  for  each  link  of  the  PUMA  robots,  forbidden  regions  are  assigned  to 
each  robot.  In  order  to  determine  the  forbidden  region  for  each  PUMA  robot,  it 
is  necessary  to  consider  the  intersection  lines  of  the  selected  plane  pair  of  one  robot 
and  that  of  the  other  robot.  As  a result,  there  are  two  forbidden  regions  on  each 
selected  plane  and  they  lie  on  the  intersection  lines  of  this  plane  and  the  selected 
pair  of  planes  of  the  other  robot.  The  forbidden  regions  are  line  segments  and  the 
determination  of  the  endpoints  of  them  is  analogous  to  that  for  the  TRS  robot. 

In  order  to  find  the  allowable  orientations  of  the  upperarm  and  the  end 
effector,  it  is  necessary  to  consider  the  plane  containing  each  of  them,  i.e.,  the 
selected  planes  ira  and  7rb  respectively.  The  upperarm  must  lie  outside  the  two 
forbidden  regions  in  the  plane  na  to  avoid  interference.  Analogously,  the  end 
effector  must  lie  outside  the  forbidden  regions  in  the  plane  7rb.  The  procedure  in 
Section  3.4.2  can  be  applied  to  each  plane  to  find  the  corresponding  allowable 
orientations  of  the  upperarm  and  the  end  effector. 

In  order  to  find  the  allowable  orientations  of  the  forearm,  the  hypothetical 
link  a 23’  (which  is  the  projection  of  the  forearm  on  plane  7rb)  introduced  in  Section 
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3.1.3  can  be  used.  As  a result,  the  mechanism  formed  becomes  a four-bar  linkage 
in  the  7rb  plane  and  the  allowable  orientations  of  the  forearm  can  be  computed 
following  the  procedure  in  Section  3.4.2. 

After  computing  the  allowable  orientations  of  each  link  of  the  PUMA  robot, 
the  favorable  orientations  can  be  determined  by  following  the  procedures  in  Section 
3.3. 


3.5  Computation  of  Joint  Angles 

In  the  previous  sections,  the  orientations  of  links  of  a robot  manipulator  are 
determined  which  are  measured  with  respect  to  a horizontal  plane.  However,  in 
order  to  control  the  motion  of  a robot,  it  is  necessary  to  determined  the  angular 
displacements  of  the  joints  of  the  robot.  The  purpose  of  this  section  is  to  derive  the 
joint  angular  displacements  from  the  orientations  of  robot  links. 

A kinematic  model  of  a TRS  robot  is  shown  in  Fig.  3.21.  The  notation  used 
in  the  analysis  is  developed  by  Duffy  (1980).  Briefly  stated,  suppose  a link  of  a 
manipulator  is  connected  by  two  kinematic  joints  with  axes  Sj  and  Sj.  The 
perpendicular  distance  between  the  joint  axes  is  a y and  the  unit  vector  along  the 
mutual  perpendicular  is  air  The  twist  angle  between  the  joint  axes  is  represented 
by  ce^  and  is  measured  in  a right  handed  sense  about  the  vector  a^. 

Further,  the  perpendicular  distance  between  two  links  connected  by  a revolute 
joint,  specifically  the  perpendicular  distance  between  the  vectors  a^  and  ajk,  is 
denoted  by  the  offset  distance  S^.  The  relative  angle  between  two  links  is 
represented  by  ©j  and  is  measured  in  a right  handed  sense  about  the  unit  vector  Sj. 
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A world  coordinate  system,  x0y0z0,  is  located  with  the  z-axis  coaxial  with  S3 
and  the  x-  and  y-axes  are  in  the  plane  of  rotation  of  a12.  For  a target  point  P in  the 
world  frame,  the  orientations  of  links  of  the  robot  are  derived  and  are  given  by  four 
parameters  $l5  $2,  $3,  and  $4.  $3  is  the  angle  measured  between  the  x-z  plane  in  the 
world  frame  and  the  n plane  where  the  upperarm  and  the  forearm  of  the  robot  lie. 
$2,  and  $3  are  respectively  the  orientations  of  the  upperarm  and  the  forearm.  is 
the  orientation  of  the  projection  of  the  end  effector  on  the  n plane.  $2,  $3,  and  $4 
are  measured  with  respect  to  the  x-y  plane  of  the  world  coordinate  system. 

The  joint  angles  02  and  03  of  the  robot  are  given  by  the  following: 

02  = $2  (3.13) 

03  = $3  - $2  + 7r/2  (3.14) 

The  determination  of  the  joint  angles  ©4  and  05  requires  the  derivation  of  the  unit 
vector  S*  of  the  axis  of  the  end  effector.  This  can  be  accomplished  by  first  finding 
the  position  of  the  wrist  joint  (the  spherical  joint)  of  the  robot.  Denoting  this  by 
Q(xq,yq,zq),  the  position  vector  of  the  wrist  joint  in  the  world  coordinate  system  is 
given  by 


Xq 

a23cos$2  + S^coss^ 

yq 

= [Rfc-fJ] 

0 

Zq  _ 

a23sin$2  + S^sinfj 

(3.15) 
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where  [R(z,-$1)]1  is  the  rotation  matrix  which  rotates  a coordinate  frame  about  the 
z-axis  by  an  angle  and  is  given  by 


[R(z>-*i)] 


cos$!  -sin$1  0 
sin$j  sin$!  0 
0 0 1 


(3.16) 


The  unit  vector  of  the  axis  of  the  end  effector  Sj®  in  the  world  coordinate 
systm  is  then  given  by 

P - Q. 

S6(0)  = (3.17) 

IP-QI 

We  associate  with  each  joint  of  the  robot  a dextral  coordinate  system;  the 
x^Zj  system  has  coordinate  vectors  ay,  S*  x ay,  Sj;  where  ij  = 12,  23,  ...  67.  The 
rotation  matrix  which  transforms  the  coordinates  of  a vector  in  the  x^Zj  system  to 
those  of  the  x^Zj  system  is  denoted  by  [Ay]  and 

[AJ  = [Rfcej)]  [R(x,aij)] 


q ^ o 

1 0 0 

q o 

0 Cy  Sy 

0 0 1 

0 -Sy  Cy 

where  q = cosQj,  Sj  = sin0j;  Cy  = cosay  and  Sy  - sinay. 


'Rotation  matrices  which  effect  rotations  about  the  x-axis  and  y-axis  by  an  angle  $ are 
represented  by  [R(x,$)]  and  [R(y,$)]. 
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The  unit  vector  of  the  axis  of  the  end  effector  S6<3)  in  the  x3y3z3  system  which 
has  coordinate  vectors  a^,  S3  x a^,  S3  is  determined  by  transforming  S6(0)  from  the 
world  coordinate  system  to  the  x3y3z3  system  and  is  expressed  by 

S,(3)  = [AJS,® 

= [AJIAJIAJS,®  (3.19) 

where  [A^]  = RKz,^)]. 

Further,  can  be  derived  by  transformed  the  unit  vector  S 6(6)  = (0,  0,  1) 
from  the  x6y6z6  system  to  the  x3y3z3  system,  and  is  given  by 

= (Xj,,  Y54,  Zm)  (3.20) 

where,  by  definition, 


^54  = X5C4  - ysS4 

Y54  = C34(x5S4  + y5C4)  - 

Z54  = S34(x5S4  + y5C4)  + CMz5 


(3.21) 


x5  = S56S5 


Y5  -(S45c56  + C45S56C5) 
X5  = C45c56  - s45s56c5 


(3.22) 


Equating  Eqs.  (3.19)  and  (3.20),  we  can  solve  for  04  and  05.  In  this  work,  since  the 
orientation  of  the  end  effector  is  considered  a free  parameter,  06  is  undefined. 

Similarly,  the  joint  angles  of  a PUMA  robot  can  be  computed.  Fig.  3.22 
shows  a kinematic  model  of  a PUMA  robot.  Note  that  due  to  the  offsets  in  the 
second  and  third  joints,  the  position  vector  of  the  wrist  joint  of  the  robot  is  given  by 
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a23cos$2  + S44COs#3 


[R(v*i)] 


S22  - S33 


a23sin$2  + S^sin^j 


(3.23) 
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66 


Fig.  3.2  PUMA  robot. 


54 


z 


Z 


(a) 


(b) 


z 


Fig.  3.3  Constraint  surface  of  wrist  joint. 


55 


Z,z' 


Fig.  3.4  Transformation  of  coordinate  system. 


(b) 


Fig.  3.5  Permisssible  orientations  of  end  effector. 


Fig.  3.6  A pencil  of  planes. 


y' 


Fig.  3.7  The  upperarm  and  the  forearm  are  on  a 
7r  plane. 
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Fig.  3.8  Constraint  curve  of  wrist. 


Fig.  3.9  Constraint  curve  of  elbow. 
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(a)  d + > a23  + s* 

d - s66  > | a23  - | 


(b)  d + s66  > a23  + s^ 
d - s66  < | a23  - | 


Fig.  3.10  Permissible  orientations  of  end  effector. 
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Fig.  3.11  The  upperarm  and  forearm  of  a PUMA  robot  lie  on  two 
parallel  vertical  planes. 
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Fig.  3.12  Planar  3R  robot  and  a line  segment  GH. 


p(78,15),  G( 50 , 30 ) , H( 70,30) 
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Fig.  3.13  Image  curve  of  relative  location  of  the  forearm  and  a line  segment. 
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Fig.  3.14  Perspective  projection  of  a face  onto  the  n plane. 


^tfT7 


Fig.  3.15  Sections  of  faces  between  planes  r and  n are  effective. 
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□ 


Fig.  3.16  Intersection  of  n plane  and  a sphere. 


63 


P - (78,15);  F = (50,30) 


-70  -60  -50  -40  -30  -20  -10  0 


X' 


Fig.  3.17  Image  curve  of  relative  positions  of  the  forearm  and  a circle. 
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Q 


Fig.  3.18  Two  robot  manipulators  lie  on  different  planes. 
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(a)  P(90,0),  elbow  up, 
wrist  up 


(b)  P(90,0),  elbow  up, 
wrist  down 


A 2 


Aa 


(c)  P(70,0),  elbow  up, 
wrist  up 


(d)  P(70,0),  elbow  up, 
wirst  down 


Fig.  3.19  Approximated  boundaries  of  links  of  a robot. 
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P:C90,0}.,  Elbow  up.  Wrist  up 


P:(90.,a;),  Elbow  up.  Wrist  down 


P : C 70, 03 , Elbow  up.  Wrist  up 


P:C70,03,  Elbow  up.  Wrist  down 
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Fig.  3.20  Exact  bound  and  approximated  bound. 
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^34 


Fig.  3.21  Kinematic  model  of  a TRS  robot. 


Fig.  3.  22  Kinematic  model  of  a PUMA  robot. 


CHAPTER  4 

INTERFERENCE  AVOIDANCE  OF  ROBOT 
MANIPULATORS  WITH  SOLID  LINKS 


In  the  previous  chapter,  the  manipulator’s  links  are  modeled  by  line  segments. 
However,  all  industrial  robot  manipulators  have  solid  links  with  cross  section 
dimensions.  Therefore,  to  be  practical,  it  is  essential  to  consider  the  manipulators 
as  consisting  of  a sequence  of  solid  links  connected  by  joints.  In  this  chapter,  the 
upperarm  and  the  forearm  of  a manipulator  are  modeled  by  parallelepipeds,  and  the 
end  effector  is  modeled  by  a cylinder. 

In  order  to  extend  the  interference  avoidance  algorithms  developed  in 
Chapter  3 to  manipulators  with  solid  links,  it  is  desirable  to  expand  the  obstacles  to 
compensate  for  the  cross  section  dimensions  of  the  links.  The  links  can  then  be 
treated  as  line  segments.  For  the  problems  of  coordination  of  two  robot 
manipulators,  the  master-slave  operations  are  solved  by  considering  the  master  robot 
as  obstacles  consisting  of  parallelepipeds  and  cylinders;  the  parallel  operations  are 
solved  by  modifying  forbidden  regions  from  line  segments  to  rectangles. 

4.1  Expansion  of  Obstacles 

The  idea  of  expanding  obstacles  to  compensate  for  the  size  of  links  appeared 
first  in  the  work  of  Udupa  [1977].  Lozano-Perez  and  Wesley  [1979]  then  used  this 
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idea  to  compute  a configuration  space  of  an  object  translating  in  space.  The 
expansion  of  obstacles  to  compensate  for  the  cross  sections  of  links  of  a spatial  robot 
can  be  found  in  Tseng  [1987],  where  a thorough  discussion  of  the  expansion  of 
obstacles  with  vertical  sides  and  flat  top  and  bottom  is  presented.  The  method  used 
for  expanding  a convex  polygon  is  to  expand  each  side  of  the  polygon  and  then 
introduces  new  sides  to  complete  the  expanded  polygon.  The  amount  of  expansion 
is  (W2  + T2)1/2/2,  where  W and  T are  the  width  and  the  thickness,  respectively,  of 
a link  of  the  robot.  This  is  equivalent  to  treating  the  robot  links  as  cylinders  with 
radius  (W2  + T2)1/2/2. 

In  this  work,  the  links  of  a TRS  robot  are  modeled  by  two  parallelepipeds 
and  one  cylinder  as  shown  in  Fig.  4.1.  The  upperarm  is  modeled  by  a parallelepiped 
of  width  Wu  and  thickness  Tu.  Similarly,  the  cross  section  dimensions  of  the  forearm 
are  represented  by  Wf  and  T,,  Since  the  upperarm  can  rotate  about  the  base  and 
shoulder  joints  while  the  forearm  can  rotate  about  the  elbow  joint,  the  vertical  sides 
of  these  two  links  will  remain  vertical  whatever  the  locations  of  these  links  are. 
The  end  effector  is  modeled  by  a cylinder  of  radius  re,  and  can  rotate  about  the  wrist 
joint  which  is  a spherical  joint. 

In  the  following,  a supplemental  discussion  of  obstacles  expansion  to 
compensate  for  the  links  of  a robot  manipulator  is  presented.  Consider  first  a 
polyhedral  obstacle  with  vertical  sides  and  horizontal  top  and  bottom.  It  can  be  seen 
from  Fig.  4.2  that  if  the  vertical  sides  of  the  obstacle  are  expanded  horizontally  along 
their  normal  directions  by  an  amount  W/2,  a link  will  not  intersect  the  obstacle  if 
the  central  line  of  the  link  does  not  intersect  the  vertical  sides  of  the  expanded 
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obstacle  , where  W is  the  transverse  dimension  of  the  link.  For  the  upperarm  and 
the  forearm  W is  the  width  of  the  arm,  for  the  end  effector  W is  the  radius  of  the 
end  effector.  Similarly,  the  expansion  of  the  top  and  bottom  faces  is  performed  by 
translating  the  faces  vertically  along  their  normal  directions  by  an  amount  T/2, 
where  T can  be  the  thickness  of  the  upperarm,  the  thickness  of  the  forearm  or  the 
radius  of  the  end  effector. 

If  the  faces  of  a polyhedral  obstacle  are  neither  vertical  nor  horizontal,  the 
normal  vector  of  each  face  has  components  on  both  vertical  and  horizontal 
directions.  In  this  case,  the  amount  of  expansion  along  the  plane  normal  is  (Wu2  + 
Tu2)1/2/2,  (Wf2  + Tf2)1/2/2,  or  re,  depending  on  which  link  is  under  consideration  (see 
Fig.  4.3). 

The  expansion  of  a spherical  obstacle  is  performed  by  increasing  the  radius 
of  the  sphere  by  an  amount  determined  by  the  cross  section  dimensions  of  robot 
links.  When  computing  the  orientations  of  the  upperarm  and  the  forearm,  the 
increment  used  is  (Wu2  + Tu2)1/2/2  and  (Wf2  + Tf2)1/2/2,  respectively.  For  the  end 
effector,  the  increment  is  the  radius  of  the  cylindrical  model,  re. 

4.2  Master-Slave  Operations  of  Two  Robot  Manipulators 

As  described  in  Section  3.4,  two  types  of  operations  for  the  coordinated 
motions  of  two  robot  manipulators  have  been  considered.  In  this  section,  allowable 
orientations  of  robot  manipulators  with  solid  links  performing  master-slave 
operations  are  discussed.  The  motion  of  the  master  robot  is  assumed  completely 
specified.  The  motion  of  the  slave  robot  is  to  be  determined  in  order  to  avoid 
interference  with  the  links  of  the  master  robot. 
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4.2.1  Allowable  Orientations  of  Upperarm 

Consider  that  the  central  lines  of  the  upperarm  and  the  forearm  of  the  slave 
robot  lie  on  a plane  of  operation  or  a n plane  (see  Fig.  4.4)  as  described  in  Chapter 
3.  In  order  to  avoid  interference  with  the  links  of  the  master  robot,  these  two  links 
need  to  avoid  interference  with  two  parallelepipeds,  which  are  used  to  model  the 
upperarm  and  the  forearm  of  the  master  robot,  and  one  cylinder,  which  is  used  to 
model  the  end  effector  of  the  master  robot. 

When  the  central  line  of  the  upperarm  is  constrained  to  lie  on  a n plane,  the 
region  swept  by  the  upperarm  is  a circular  region  with  radius  equal  to  the  distance 
from  the  axis  of  rotation  to  the  end  of  the  link  and  thickness  equal  to  the  width  of 
the  link.  This  region  is  denoted  by  nu  (Fig.  4.5).  If  the  links  of  the  master  robot  lie 
within  this  region,  the  upperarm  of  the  slave  robot  may  interfere  with  the  master 
robot. 

In  order  to  compute  the  allowable  orientations  of  the  upperarm  of  the  slave 
robot,  it  is  essential  to  find  the  intersection  of  the  links  of  the  master  robot  and  the 
circular  region  nu.  In  the  following,  the  determination  of  intersection  regions  of  nu 
and  the  links  of  the  master  robot  is  presented. 

4.2. 1.1  Parallelepipeds 

When  a parallelepiped  intersects  with  the  circular  region  nu  as  shown  in  Fig. 
4.6,  the  intersection  region  can  be  bounded  by  a box  with  length  1^,  width  Wb  and 
height  Hb.  A top  view  of  the  intersection  is  shown  in  Fig.  4.7.  It  is  worth  to  recall 
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that  the  vertical  sides  of  the  upperarm  and  the  forearm  of  the  robots  remain  vertical 
whatever  the  locations  of  the  links  are.  Let  Wu  represent  the  width  of  the  upperarm 
of  the  slave  robot  and  Wp  represent  the  width  of  the  parallelpiped.  The  length  of  the 
intersection  box  is  then  given  by 
Wu  Wp 

4 = + (4.1) 

tana  sina 

where  a is  the  angle  between  the  n plane  and  a vertical  plane  passing  through  the 
central  line  of  the  parallelepiped.  Analogously,  the  height  of  the  intersection  region 
can  be  derived  and  is  given  by 

Tr 

Hb  = Wutan/3  + (4.2) 

cos/3 

where  Tp  is  the  thickness  of  the  parallelepiped  and  /3  is  the  angle  of  the 
parallelepiped  with  respect  to  a horizontal  plane.  Wb  is  equal  to  Wu. 

In  order  to  determine  the  allowable  orientations  of  the  upperarm  in  the 
presence  of  the  parallelepiped,  consider  first  the  sections  of  the  intersection  box  and 
the  upperarm  of  the  slave  robot  on  the  n plane.  The  section  of  the  intersection 
box  on  the  ir  plane  is  a rectangle  with  dimensions  L„  and  Hb.  The  section  of  the 
upperarm  of  the  slave  robot  on  the  tt  plane  is  also  a rectangle  with  dimensions  a12 
and  Tu,  where  au  is  the  link  length  of  the  upperarm.  As  shown  in  Fig.  4.8,  the 
rectangle  ABCD  represents  the  section  of  the  intersection  box  on  the  n plane  and 
the  rectangle  GHIJ  represents  the  section  of  the  upperarm  of  the  slave  robot  on  the 
n plane.  A reference  coordinate  system  is  chosen  such  that  the  x-z  plane  coincides 
with  the  7r  plane,  and  the  x axis  is  parallel  to  a horizontal  plane. 
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Further,  the  section  of  ftu  in  the  ir  plane  is  a circle  centered  at  the  shoulder 
O and  with  radius  [au2  + (Tu/2)2]1/2  and  is  denoted  by  Cu.  The  section  of  Cu  outside 
rectangle  ABCD  can  be  expressed  in  terms  of  an  angular  range  [a1}  a2],  where  al5 
a2  are  the  angles  between  the  x axis  and  the  points  of  intersection  of  Cu  and 
rectangle  ABCD  (Fig.  4.9)  or  the  angles  between  the  x axis  and  the  vertices  of 
rectangle  ABCD  (Fig.  4.10). 

Finally,  the  angular  range  [ax,  a2]  is  narrowed  to  compensate  for  the  thickness 
of  the  upperarm  (rectangle  GHIJ)  and  is  given  by  [aj  + <S1(  a2  - 62],  where  61  and 
S2  can  have  values  of  tan_1[(Tu/2)  / a12]  or  tan'‘[(Tu/2)  / dv],  depending  on  how 
and  a2  are  determined.  If  aj  (or  a2)  is  determined  by  the  angle  between  the  x axis 
and  a point  of  intersection  of  Cu  and  rectangle  ABCD,  <5,  (or  S2 ) = tan‘1[(Tu/2)  / 
aj.  Otherwise,  6,  (or  S2 ) is  determined  by  the  angle  between  the  x axis  and  a vertex 
of  rectangle  ABCD,  (or  S2 ) = tan'1[(Tu/2)  / dj,  where  dv  is  the  distance  of  the 
vertex  of  rectangle  ABCD  to  the  origin  O. 

[o-j  + a2  - <S2]  gives  the  orientation  range  of  rectangle  GHIJ  in  which  it 
does  not  intersect  with  rectangle  ABCD.  The  allowable  orientations  of  the 
upperarm  are  given  by  the  intersection  of  the  permissible  orientations  of  the 
upperarm  and  the  angular  region  [a2  + <51(  a2  - <S2]. 

4.2.1.2  Cylinders 

When  a cylinder  intersects  with  the  circular  region  nu  (see  Fig.  4.11),  it  is 
convenient  to  consider  the  intersections  of  the  cylinder  and  the  two  vertical  sides  of 
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nu.  Denoted  by  ttx  and  n2  the  two  planes  containing  the  two  vertical  sides  of  nu.  It 
is  desirable  to  find  the  intersection  of  the  cylinder  with  the  planes  n1  and  n2. 

The  intersection  of  a cylinder  and  a plane  is,  in  general,  an  ellipse.  The 
ellipse  can  be  considered  as  a perspective  projection  of  a circle  from  a point  at 
infinity  onto  the  plane.  The  equation  of  the  ellipse  can  be  derived  in  a similar 
manner  to  the  derivation  of  the  plane  section  of  a cone  given  in  Section  2.4. 

In  the  following,  the  orientations  of  the  upperarm  in  which  it  does  not 
intersect  with  the  ellipses  on  the  ni  and  n2  planes  are  computed.  As  shown  in  Fig. 
4.12,  the  rectangle  GHIJ  with  dimensions  an  and  Tu  represents  the  section  of  the 
upperarm  of  the  slave  robot  in  the  ttj  (or  ir2)  plane.  The  ellipse  represents  a section 
of  a cylinder  in  the  flq  (or  n2)  plane.  Using  a translation  followed  by  a rotation  of 
the  coordinate  system,  the  ellipse  can  be  transformed  into  an  isothetic  ellipse 
centered  at  the  origin  of  the  new  coordinate  system. 

In  order  to  determine  the  intersection  of  a rectangle  and  an  ellipse,  two 
methods  are  presented.  The  first  method  uses  an  expansion  of  ellipses  so  that  a 
rectangle  can  be  treated  as  a line  segment.  The  second  method  uses  an  an 
expansion  of  circles  after  applying  a linear  transformation  to  map  ellipses  into 
circles. 


Expansion  of  ellipses.  An  ellipse  centered  at  the  origin  whose  major  and 
minor  axes  are  isothetic  can  be  expressed  by 
x2  y2 


= 1 


+ 


(43) 


75 


where  a and  b are  the  lengths  of  the  semi-major  and  semi-minor  axes  respectively, 
and  its  parametric  form  is 

x = acos©  (4.4a) 

y = bsine  (4.4b) 

In  order  to  determine  if  a rectangle  with  dimensions  a12  and  T and  varying 
orientations  will  intersect  an  ellipse  of  the  form  in  Eq.  (4.3),  the  ellipse  is  expanded 
by  an  amount  T/2  and  the  rectangle  is  treated  as  a line  segment.  This  expansion 
is  equivalent  to  an  actual  physical  space  covered  by  a circle  of  radius  T/2  when  the 
center  of  the  circle  is  on  the  ellipse  (Fig.  4.13).  The  expanded  figure,  denoted  by 
EF,  thus  obtained  has  the  parametric  form 

(T/2)  -bcos© 

x = acos0  + (4.5a) 

(a2sin20  + b2cos20)1/2 

(T/2)  -asm© 

y = bsin©  + (4.5b) 

(a2sin20  + b2cos20)1/2 

where  0 < 0 < 2n. 

A complete  derivation  of  the  above  figure  together  with  some  important 
characteristics  of  this  figure  can  be  found  in  the  work  of  Oommen  and  Reichstein 
(1987).  Some  characteristics  of  this  figure  which  are  useful  for  this  work  are 
summarized  in  the  following, 

(1)  For  any  parameter  0,  the  slope  of  the  tangent  line  to  the  ellipse  in  Eq.  (4.4) 
has  exactly  the  same  value  as  the  slope  of  the  tangent  to  the  expanded  figure 
in  Eq.  (4.5)  at  the  corresponding  point. 
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(2)  For  the  parameter  0,  the  equation  for  the  tangent  line  to  the  expanded  figure 

has  the  form 

xcos©  ysin0  (T/2)  • (a2sin20  + b2cos20)1/2 

+ = 1 + (4.6) 

a b ab 

(3)  The  slope  m of  the  tangent  to  the  expanded  figure  passing  through  the  point 

(x,y)  has  the  value  -1/t,  where  t is  a root  of  the  quartic  equation: 

k4t4  + k3t3  + k2t2  + kjt  + lq,  = 0 (4.7) 

where,  if  F(p,q,r)  = p2  - q2  - r2,  and  G(p,q)  = 2pq, 
k4  = F2(y,b,T/2)  - G2(b,T/2) 
k3  = 2 -F(y,b,T/2)  -G(x,y) 

k2  = 2 *F(y,b,T/2)  -F(x,a,T/2)  + G2(x,y)  - G2(a,T/2)  - G2(b,T/2) 

K = 2-F(x,a,T/2)-G(x,y) 
k0  = F2(x,a,T/2)  - G2(a,T/2) 

By  using  the  expansion  of  the  ellipse  in  the  7r,  (or  n2)  plane,  the  problem  of 
interest  becomes  finding  the  orientations  of  a line  segment  fixed  in  one  end  point 
to  avoid  intersection  with  the  expanded  figure  EF.  When  one  end  point  of  a line 
segment  is  fixed,  the  area  swept  by  the  line  segment  is  a circular  region  Cu  (Fig. 
4.14).  The  orientations  of  the  line  segment  in  which  it  intersects  EF  are  determined 
either  by  tangents  of  EF  passing  through  the  fixed  end  point  or  the  intersection 
points  of  Cu  and  EF. 

When  the  points  of  tangency  of  EF  are  outside  C„,  the  orientation  range  of 
interference  is  determined  by  the  points  of  intersection  of  EF  and  Cu.  Otherwise, 
it  is  determined  by  the  points  of  tangency  of  EF.  The  tangents  of  EF  passing 
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through  the  fixed  end  point  can  be  determined  by  solving  Eq.  (4.7).  However,  the 
intersection  of  Cu  and  EF  cannot  be  obtained  analytically.  Therefore,  if  the  points 
of  tangency  of  EF  are  outside  Cu,  the  orientations  of  the  intersection  points  of  Cu 
and  EF  are  approximated  by  the  orientations  of  the  tangent  lines  of  EF  passing 
through  the  center  of  Cu.  The  applicability  of  this  approximation  is  due  to  the  fact 
that  Cu  which  has  a radius  equal  to  a link  length  is  substantially  bigger  than  EF 
which  has  axes  lengths  in  the  orders  of  the  cross  section  dimensions  of  a link. 

Transformation  of  ellipses.  Using  the  transformation  of  ellipses  described  in 
Section  2.4,  the  problem  of  determining  the  intersection  of  a rectangle  and  an  ellipse 
can  be  transformed  into  the  problem  of  determining  the  intersection  of  a 
parallelogram  and  a circle.  This  problem  is  further  reduced  to  the  problem  of 
finding  the  intersection  of  a line  segment  and  a circle  and  can  be  solved  by  using  the 
method  presented  in  Section  2.2.  Note  that  using  this  transformation,  the  orientation 
0 of  a line  segment  with  respect  to  the  x axis  of  the  reference  frame  is  transformed 
into  0’,  and  they  are  related  by 

tan0’  = (Oy/cjx)  tan0  (4.8) 

where  ax  and  ay  are  as  those  defined  in  Section  2.3.  As  far  as  the  orientation  of  the 
line  segment  is  concerned,  this  conversion  should  be  applied  appropriately. 

After  finding  the  intersection  of  the  upperarm  and  the  plane  sections  of  the 
cylinder  in  the  7r,  and  n2  planes,  it  is  necessary  to  find  the  intersection  of  the 
upperarm  and  the  cylinder  itself.  Suppose  the  orientation  ranges  of  the  upperarm 
within  which  it  has  intersection  with  the  plane  sections  of  a cylinder  in  the  n1  and 
n 2 planes  are  given  by  [/q,  /q]  and  [m3,  m4],  respectively.  The  orientation  range  of  the 
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upperarm  within  which  it  intersects  with  the  cylinder  is  given  by  [Mi,  M4]-  The 
allowable  orientations  of  the  upperarm  is  then  obtained  by  excluding  [/il5  /x4]  frome 
the  permissible  orientaions. 

4.2.2  Allowable  Orientations  of  Forearm 


Since  the  forearm  and  the  upperarm  always  lie  on  the  same  plane,  the  central 
line  of  the  forearm  remains  in  the  n plane  where  the  central  line  of  the  upperarm 
locates  (see  Fig.  4.4).  In  addition,  the  end  points  of  the  forearm  are  constrained 
by  the  elbow  and  wrist  joints  which  move  in  circular  arcs.  In  order  to  find  the 
allowable  orientaions  of  the  forearm,  the  analysis  for  the  upperarm  described  above 
can  be  applied  with  some  modifications.  In  the  following,  the  determination  of 
allowable  orientations  of  the  forearm  with  respect  to  the  links  of  the  master  robot 
which  are  modeled  by  parallelepipeds  and  cylinders  are  presented. 

4.2.2. 1 Parallelepipeds 


When  the  central  line  of  the  forearm  lies  on  a tt  plane,  the  two  vertical  sides 
of  the  forearm  lie  on  two  vertical  planes  which  are  denoted  by  and  n2.  Denoted 
by  nf  the  region  between  planes  n1  and  n2  (see  Fig.  4.15).  The  intersection  of  nt  and 
a parallelepiped  can  be  bounded  by  a box  with  dimensions  L^,,  Wb  = Wf  and  Hb,  and 
1^,  and  Hb  are  given  by 


4 = 


wf 


w„ 


tana 


+ 


sina 


(4.9) 
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Hb  = Wftan/3  + (4.10) 

cos/3 

where  Wf  is  the  width  of  the  forearm,  Wp  and  Tp  are  width  and  thickness  of  the 
parallelepiped,  a is  the  angle  between  the  n plane  and  a vertical  plane  passing 
through  the  central  line  of  the  parallelepiped,  and  /3  is  the  angle  of  the 
parallelepiped  with  respect  to  a horizontal  plane. 

In  order  to  determine  the  allowable  orientations  of  the  forearm,  it  is  helpful 
to  consider  the  plane  sections  of  the  above  bounding  box  and  the  forearm  of  the 
slave  robot  on  the  it  plane.  As  a result,  the  forearm  is  represented  by  a rectangle 
with  dimensions  a^,  Tf,  and  the  parallelepiped  is  reduced  to  a rectangle  with 
dimensions  1^  and  Hb. 

The  allowable  orientations  of  the  forearm  can  be  obtained  by  considering  the 
forearm  as  a line  segment  while  the  rectangle  is  expanded  by  T,/2  along  the  normal 
direction  of  each  side  to  compensate  for  the  thickness  of  the  forearm.  Using  the 
method  described  in  Section  3.2.1,  the  allowable  orientations  of  the  forearm  with 
respect  to  the  expanded  rectangle  can  be  obtained. 

4.2.2.2  Cylinders 

A cylinder  intersects  with  the  tt1  and  n2  planes  in  two  ellipses  (see  Fig.  4.16). 
In  order  to  determine  the  allowable  orientations  of  the  forearm  in  the  presence  of 
a cylinder,  the  orientations  of  the  forearm  in  which  it  does  not  intersect  with  the 
ellipses  on  the  zTj  and  n2  planes  are  determined. 
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The  intersections  of  the  forearm  and  an  ellipse  on  the  n1  (or  n2)  plane  is 
equivalent  to  the  intersections  of  the  plane  section  of  the  forearm  on  the  irl  (or  n2) 
plane  and  the  ellipse  on  the  same  plane.  The  determination  of  thses  intersections 
can  be  accomplished  by  a transformation  of  ellipse  followed  by  a determination  of 
intersections  of  line  segments  and  a circle  as  described  in  Section  4.2. 1.2. 

4.2.3  Allowable  Orientations  of  End  Effector 

When  the  upperarm  and  the  forearm  of  the  slave  robot  lie  on  a plane,  its  end 
effector  lies  on  a circular  cone.  The  allowable  orientations  of  the  end  effector  of  the 
slave  robot  are  determined  by  considering  the  intersection  of  a cone  and  the  links 
of  the  master  robot. 

Before  finding  the  intersection  of  the  end  effector  of  the  slave  robot  and  the 
links  of  the  master  robot,  it  is  necessary  to  detect  whether  the  intersection  is  possible 
or  not.  In  order  to  detect  the  possibility  of  intersection  between  the  end  effector  of 
the  slave  robot  and  the  links  of  the  master  robot,  it  is  convenient  to  consider  the 
links  of  the  master  robot  as  cylinders.  The  radii  of  the  cylinders  are  as  small  as 
possible  while  the  cylinders  encompass  the  links.  Further,  these  cylinders  are 
expanded  by  the  radius  of  the  end  effector  of  the  slave  robot.  The  detection  of  the 
possibility  of  intersection  of  the  end  effector  of  the  slave  robot  and  the  links  of  the 
master  robot  can  then  be  performed  by  finding  the  relative  location  of  a cone  and 
a cylinder  as  described  in  Section  2.5. 

The  upperarm  and  the  forearm  of  the  master  robot  are  modeled  by 
parallelepipeds.  In  order  to  compute  the  allowable  orientations  of  the  end  effector 
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of  the  slave  robot  with  respect  to  the  upperarm  and  the  forearm  of  the  master  robot, 
the  intersection  of  a cone  and  a parallelepiped  is  considered,  which  can  be  found  in 
Section  3.2.1.  However,  before  applying  that  algorithm,  it  is  essential  to  expand  the 
parallelepiped  along  the  normal  direction  of  each  side  by  the  radius  of  the  end 
effector  of  the  slave  robot.  Similarly,  in  order  to  compute  the  allowable  orientations 
with  respect  to  the  end  effector  of  the  master  robot,  the  intersection  of  a cone  and 
a cylinder  is  needed,  which  can  be  found  in  Section  2.5. 

Consequently,  by  computing  the  intersection  of  allowable  orientations  with 
respect  to  each  link  of  the  master  robot,  the  allowable  orientations  of  the  end 
effector  of  the  slave  robot  can  be  obtained. 

4.3  Parallel  Operations  of  Two  Robot  Manipulators 

In  this  section,  the  problem  of  interference  avoidance  of  two  manipulators 
operating  in  parallel  is  investigated.  Recall  that  in  Section  3.4.2,  robot  manipulators 
are  considered  having  4 DOF,  and  their  links  remain  in  a plane  as  their  motion 
proceeds.  And  the  problem  was  solved  by  using  forbidden  regions  which  were 
represented  by  line  segments.  This  section  modifies  the  development  in  Section  3.4.2 
so  it  can  be  applied  to  robot  manipulators  with  solid  links. 

At  any  instant,  the  links  of  a robot  manipulator  are  restricted  in  a vertical 
region.  Let  the  regions  where  the  two  robots  lie  be  denoted  by  ^ and  fl2.  The 
respective  widths  of  these  two  regions  are  represented  by  Wx  and  W2.  Further,  two 
vertical  planes  which  lie  in  the  center  of  r2x  and  n2  are  represented  by  i ri  and  n2, 
respectively.  It  is  clear  that  the  only  possible  location  for  interference  is  somewhere 
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within  the  intersection  prism,  E,  of  these  two  regions  as  shown  in  Fig.  4.17.  It  is  thus 
necessary  to  determine  a forbidden  region  for  each  robot  within  the  intersection 
prism  Z. 

The  intersection  prism  2 on  the  regions  and  n2  can  be  bounded  by  boxes 
B1  and  B2  with  lengths  and  1^,  and  widths  Wbl  and  Wb2, 


wx 

w2 

W = 

+ • 

(4.11) 

tana 

sina 

W2 

wx 

Lb2  = 

+ - 

(4.12) 

tana 

sina 

wbl  = 

wx 

(4.13) 

wb2  = 

w2 

(4.14) 

where  a is  the  angle  between  7rJ  and  ir2  planes.  The  plane  sections  of  Bj  on  the 
plane  n1  is  a rectangle  with  width  L^.  Similarly,  the  plane  section  of  B2  on  the  plane 
n2  is  a rectangle  with  width  L^. 

The  forbidden  regions  of  two  robot  manipulators  can  be  determined  by 
considering  the  plane  sections  of  Bj  and  B2  and  the  links  of  the  two  manipulators  on 
the  planes  nl  and  tt2,  respectively.  The  procedure  for  determining  the  forbidden 
regions  is  described  in  the  following. 

First,  determine  forbidden  regions  of  two  line-segment-modeled  manipulators 
lying  on  the  n1  and  n2  planes  by  using  the  procedure  discribed  in  Section  3.4.2.  This 
will  yield  two  disjoint  forbidden  regions  which  are  line  segments  in  the  intersection 
line,  /,  of  7Tj  and  n2  planes.  However,  before  applying  this  procedure,  a modification 
has  to  be  made  in  order  to  compensate  for  the  thicknesses  of  robot  links.  This  is 
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because  when  a link  of  a robot  manipulator  passes  through  the  end  points  of  the 
bounded  region  in  the  intersection  line  /,  the  bounded  region  needs  to  contain  the 
thickness  of  the  link.  The  length  of  the  additional  bounded  region  in  the  line  / is 
dependent  on  the  orientation  of  the  link  and  can  be  expressed  as  follows: 

d = T / (2cos/?)  (4.15) 

where  d is  the  length  of  additional  bounded  region,  T is  the  thickness  of  the  link  and 
f3  is  its  orientation  with  respect  to  the  horizontal  plane  (see  Fig.  4.18).  It  is 
important  to  recognize  that  the  bounded  region  in  line  / are  union  of  regions  of 
intersection  of  links  of  the  manipulator  with  line  /.  Therefore,  in  order  to  find  the 
new  bounded  regions  in  /,  each  link’s  thickness  has  to  be  considered. 

Second,  the  two  line-segment  forbidden  regions  on  the  n1  and  n2  planes  are 
expanded  to  rectangles  which  have  widths  of  and  L„2,  respectively.  The  heights 
of  the  rectangles  are  the  same  as  the  lengths  of  the  two  line-segment  forbidden 
regions. 

Finally,  the  problem  reduces  to  the  determination  of  the  orientations  of  links, 
which  are  rectangles,  of  a manipulator  such  that  the  links  do  not  interfere  with  a 
rectangular  forbidden  region.  The  method  used  for  solving  this  problem  can  be 
found  in  the  discussion  of  master-slave  operations  of  two  robot  manipulators  in  the 
previous  section. 
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Fig.  4.1  Robot  manipulators  with  solid  links. 


Fig.  4.2  Expansion  of  vertical  sides. 


Fig.  4.3  Expansion  of  oblique  sides. 
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Fig.  4.4  The  central  lines  of  the  upperarm  and  the  forearm 
lie  on  the  ir  plane. 


Fig.  4.5  The  upperarm  sweeps  a region  nu. 
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Z 


Fig.  4.6  Intersection  region  of  nu  and  a parallelepiped. 


y 


Fig.  4.7  Top  view. 


Fig.  4.8  Plane  section  of  the  upperarm  and  an  intersection  box. 


Fig.  4.10  Vertices  of  rectangle  inside  Cu. 
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Z 


Fig.  4.11  A cylinder  intersects  with  f2u. 
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Fig.  4.12  Plane  sections  of  the  upperarm  and  a cylinder. 
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y 


Fig.  4.13  Expansion  of  an  ellipse. 


Fig.  4.14  Intersection  of  the  expanded  figure  (EF)  and  Cu. 
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Fig.  4.15  Intersection  of  a parallelepiped  and  fif  is  bounded  by  a box. 


plane 


Fig.  4.16  Intersection  of  a cylinder  and  nf. 
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Fig.  4.17  Possible  location  of  interference  of  two  robot  manipulators. 
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Fig.  4.18  Determination  of  the  length  of  the  additional  bounded  region. 


CHAPTER  5 

TIME  SCHEDULING  OF  TWO  ROBOT  MANIPULATORS 


In  Chapters  3 and  4,  the  determination  of  favorable  orientation  ranges  of  two 
robot  manipulators,  which  is  based  on  the  predefined  trajectories  of  the 
manipulators,  has  been  discussed.  However,  it  is  possible  that  the  robot 
manipulators  may  not  have  favorable  orientations  at  some  instants  during  the 
motion.  This  implies  that  the  two  manipulators  cannot  avoid  each  other  while 
tracking  their  specified  trajectories.  In  this  situation,  the  trajectories  should  be 
modified  such  that  the  robot  manipulators  can  arrive  at  their  goal  positions 
successfully. 

In  this  section,  it  is  proposed  to  use  speed  reduction  or  time  delay  of  either 
one  of  the  two  robot  manipulators  without  changing  its  path  to  generate  an 
interference-free  motion.  In  order  to  do  this,  two  tasks  must  be  performed: 

1.  determine  potential  collisions  between  the  links  of  the  two  robot  manipulators 
according  to  the  specified  paths. 

2.  modify  the  trajectories  of  the  manipulators  such  that  the  potential  collisions 
can  be  avoided. 

In  the  following,  potential  collisions  between  the  links  of  a pair  of  robot 
manipulators  are  discussed.  The  potential  collisions  are  assumed  to  happen  only 
between  the  end  effectors  which  are  moving  on  straight  line  paths.  Strategies  for 
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modifying  trajectories  of  robot  manipulators  for  the  purpose  of  avoiding  potential 
collisions  are  then  proposed. 

The  approach  presented  here  is  somewhat  similar  to  the  work  of  Lee  and  Lee 
(1987).  It  is  important  to  recognize  that  in  their  work,  the  boundary  of  the  potential 
collision  region  was  not  discribed  analytically  and  was  approximated  by  a bounding 
box.  However,  here  a complete  novel  analysis  is  presented  where  the  potential 
collision  region  is  described  precisely. 

5.1  Potential  Collision  Regions 

Since  the  end  effector  is  instantaneously  joined  to  a fixed  target  point,  the 
permissible  locations  of  an  end  effector  lie  within  a sphere  and  it  is  reasonable  to 
model  the  end  effector  by  a sphere.  The  center  of  the  sphere  coincides  with  the 
target  point  on  the  specified  path  and  the  radius  of  the  sphere  is  equal  to  the  length 
of  the  end  effector  (see  Fig.  5.1).  Using  this  model,  the  interference  problem  is 
reduced  to  determining  the  intersection  of  a pair  of  spheres. 

Let  the  parametric  equations  of  two  straight  lines  which  represent  the  paths 
of  the  end  effectors  of  two  manipulators  be  given  by 

Ei  = U,  + s1(V1  - Uj)  0 < s,  < 1 (5.1) 

E2  = U2  + S2(V2  - U2)  0 < s2  < 1 (5.2) 

where  U,  and  V,  are  beginning  and  end  points  of  the  ith  manipulator.  It  is  assumed 
that  there  is  no  interference  between  the  two  end  effectors  when  they  are  at  the 
beginning  and  end  points  of  the  straight  line  paths. 
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The  intersection  of  a pair  of  spheres  occurs  when  the  distance  between  the 
centers  of  the  spheres  is  less  than  the  sum  of  radii  of  the  two  spheres.  Thus,  a 
potential  collision  region  can  be  obtained  and  the  boundary  of  the  potential  collision 
region  is  determined  by  the  following  equation: 

IE1-E2I  = E + r2  (5.3) 

where  rx  and  r2  are  link  lengths  of  the  two  end  effectors.  Replacing  Px  and  P2  by 
Eqs.  (5.1)  and  (5.2)  yields 

I (III  - 10  + s,(V,  - 10  - s,(Y,  -12,)  | = r,  + r,  (5.4) 

Square  both  sides  of  Eq.  (5.4)  yields 


IY,  - li,|  V - 2(V,  - 12,)  (Y,  - U,)s,s2  + |V,  -12,1V 
+ 2(12,  - iO ' (Y,  - U,)s,  - 2(12,  - IO  • (Y,  - 10% 


+ 1 lii  - u2 1 2 - (r,  + r2)2  = 0 

(5.5) 

which  can  be  expressed  in  the  form 

Asj2  - 2Bs!S2  + Cs22  + 2Ds:  - 2Es2  + F = 0 

(5.6) 

which  is  a quadratic  equation  in  sx  and  s2.  The  discriminant  of  the  second-degree 
equation  is  given  by 

B2-  AC  = [(Vj  - UJ  • (V2  - U2)]2  - | Vj  - Uj  | 2 1 V2  - U2 1 2 
= I Yi  - Ui  1 2 1 Y2  - U2 1 2(cos2a  - 1) 

< 0 

where  a is  the  angle  between  the  vectors  V1  - U,  and  Y2  - U2. 

When  B2  - AC  < 0,  the  trace  of  the  second-degree  equation  in  the  parametric 
space,  i.e.,  the  s2-sx  plane,  is  an  ellipse.  When  B2  - AC  = 0,  i.e.  cosa  = ±1,  the  trace 
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is,  in  general,  a parabola.  However,  in  the  present  case,  when  B2  - AC  = 0,  the 
equation  is  degenerate  and  reduces  to  two  parallel  lines  which  are  represented  by 
( I Yj  - Uj  | Sj  - cosa  | V2  - U2 1 s2  + | Uj  - U2 1 cos/3)2 
= Oh  + r2)2  - ( | Uj  - U2 1 sin/3)2  (5.7) 

where 

Oi  - id  • (v,  - id 

/3  = cos1 (5.8) 

|U1-U2|  IV,  -HJ 

In  the  following  discussion,  ellipses  will  be  used  to  represent  the  potential  collision 
regions. 

It  is  further  assumed  that  the  robot  manipulators  are  performing  motions 
which  involve  three  stages:  acceleration,  constant  velocity,  and  deceleration,  and  the 
velocity  vs.  time  graph  is  shown  in  Fig.  5.2.  For  this  analysis,  the  acceleration  and 
deceleration  of  each  manipulator  are  assumed  to  be  the  same  in  magnitude. 

Using  these  velocity  profiles,  the  postition  of  each  robot  at  a given  time  can 
be  determined.  The  traveled  distances  of  the  robots  in  terms  of  the  parameters  s, 
and  s2  at  each  time  interval  can  be  expressed  as  follows: 

(A)  Robot  1 

(1)  t0  < t < t, 

a,t2 

2 1 V,  - U,  | 

(2)  t,  < t < t4 

aih(t  - t,) 

Si  = s,(t,)  + 

lYi-UJ 


(5.9a) 


(5.9b) 
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where 

aiti2 

si(li)  = 

2 1 Yx  - Uj  | 

(3)  t4  < t < tf 

ai(tf  - 1)2 

Sj  = 1 (5.9c) 

2 1 Vj  - Uj  | 


(B)  Robot  2 
(1)  t0  < t < t2 

a2t2 


2 1 Y2  - H2 1 

(2)  t2  < t < t3 

a2t2(t  - t2) 

s2  = s2(t2)  + 

lY2-U2| 

where 

a2t22 

s2(l2)  = 

2 1 Y2  - U2 1 

(3)  t3  < t < tf 

a2(tf  - t)2 

s2  = 1 

2|Y2-U2| 


According  to  the  above  trajectories,  the  corresponding  positions  between  the 
two  robots  at  various  time  intervals  are  computed  by  eliminating  the  time  from  Eqs. 
(5.9)  and  (5.10)  and  are  shown  in  the  following: 


(5.10a) 


(5.10b) 


(5.10c) 
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(1)  t0  < t < tx 

ajY.-Uj 

s2  — Sj  — AjSj  (5.11a) 

ajY^Uj 

(2)  tx  < t < t2 

s2  - s2(tx)  — BjfSj  - Sj(tj)]  + B2[Sj  - s1(t1)]  (5.11b) 

where 

a2ti2 

s2(ti)  = 

2 1 V2  - U2 1 

B,  = A, 

l Yi  - Hi  1 2 

B2  — a2 

2(a1t1)2 1 V2  - U2 1 

(3)  t2  < t < t3 


s2  - s2(t2)  CjfSj  - Sj(t2)] 


(5.11c) 


where 


aiti(t2  - t,) 

Si(t2)  = Sjfo)  + 

I Yi  - Uj  | 

a2t2 1 Yi  - Hi  I 

Q = 

a1t1lY2-U2| 

(4)  t3  < t < t4 


s2  - s2(t3)  = DJs,  - sx ( t3) ] + D2[Sl  - S3(t3)]2  (5.1  Id) 

where 


aitx(t3  - t2) 
Si(t,)  = s,(t2)  + 


IY.-U.I 
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a2^2(U  ' ^2) 

s2(t3)  = s2(t2)  + 

IY2-U2| 

Dj  = Q 

D2  = -B2 

(5)  t4  < t < tf 

s2  - 1 = E^Sj  - 1)  (5. lie) 

where  Ei  = A1 

Consequently,  a curve  in  the  parametric  space  is  obtained  and  is  illustrated 
in  Fig.  5.3.  If  this  curve  is  outside  the  potential  collision  region,  the  two  end 
effectors  will  not  collide  with  each  other.  Otherwise,  the  intersection  points  of  this 
curve  with  the  potential  collision  region  must  be  identified. 

In  order  to  visualize  the  effect  of  modification  of  robot  trajectories,  it  is 
convenient  to  map  the  potential  collision  region  from  Sj-s2  plane  to  either  s2-t  or  s2- 
t plane.  This  is  performed  by  replacing  s2  or  Sj  in  Eqs.  (5.11)  by  Eqs.  (5.10)  or  (5.9) 
for  each  time  interval.  As  a result,  a closed  region  is  obtained,  and  the  boundaries 
of  this  region  at  different  time  intervals  are  given  as  follows: 

(A)  Sj-t  plane 

(1)  t0  < t < t2 

Asj2  - ZBpjSjt2  + C(pp2)2  + 2Dsj  - 2Epjt2  + F = 0 
a2 

where  pj  = 

2|V2- VJ 

(2)  t2  < t < t3 

Asi2  - 2Bp2Sjt  + C(p2t)2  + 2(D  - Bq2)s1  - 2p2(E  - Cq^t 


(5.12a) 
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+ F + Cq/  - 2Eq_2  = 0 
a2t2 


where  p2  = 


IV,  - Si. 


a2t2 


q 2 = s2(t2)  - - 

|v2-u2i 

(3)  t3  < t < tf 

As/  - 2Bsx(p3t2  + q3t  + r3)  + C(p3t2  + q3t  + r3)2 
+ 2Ds3  - 2E(p3t2  + q3t  + r3)  + F = 0 
a2 

where  p3  = 

|y2-u2i 

a2tf 


q3  = 


|v2-u2l 


r,  = 1 - 


a2tf 


2 1 V2  - U, 


(B)  s2-t  plane 

(1)  t0  < t < tj 

A(P4t2)2  - 2Bp4s2t2  + Cs22  + 2Dp4t2  - 2Es2  + F = 0 
a, 


where  p4  = 


2 1 V 3 - Uj  | 

(2)  tx  < t < t4 

A(p5t)2  - 2Bp5s2t  + Cs22  + 2p5(D  + Aqj)t 

-2(E  + Bq5)s2  + F + Aq52  + 2Dq5  = 0 


(5.12b) 


(5.12c) 


(5.13a) 


(5.13b) 
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where  p5  = 

lY,-U,| 

a,!,2 

= s,(t,)  

IV, -UJ 

(3)  t4  < t < tf 

A(p6t2  + q6t  + r6)2  - 2B(p6t2  + q6t  + r6)s2  + Cs22 

+ 2D(p6t2  + q6t  + r6)  - 2Es2  + F = 0 (5.13c) 

where  p6  = 

2 1 Y,  - U,  | 


I Y,  - U,  | 
a,tf2 

r6  = 1 * 

2 1 V,  - U,  | 

The  modification  of  the  robots  trajectories  can  be  done  conveniently  either 
in  the  s,-t  plane  or  in  the  s2-t  plane,  depending  upon  which  robot  trajectory  is  to  be 
modified  (see  Fig.  5.4). 


5.2  Modification  of  Trajectories 

When  the  distance  curve  of  two  end  effectors  in  the  parametric  space 
intersects  with  the  potential  collision  region,  these  two  end  effectors  will  collide  with 
each  other  while  tracking  their  specified  trajectories.  In  order  to  avoid  collision,  the 
trajectory  of  either  one  of  the  two  robot  manipulators  can  be  modified. 
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Two  methods  for  modifying  the  trajectories  of  robots  are  discussed  here. 
The  first  method  is  to  delay  the  motion  of  a robot  to  avoid  potential  collisions,  and 
the  second  method  uses  a speed  reduction  concept. 

Before  discussing  in  detail  about  these  methods,  it  is  helpful  to  know  which 
robot  trajectory  can  be  modified.  In  general,  both  robot  trajectories  are  considered 
modifiable.  In  some  cases,  however,  it  is  preferable  to  alter  the  trajectory  of  one 
robot  rather  than  the  other. 

Consider  different  locations  of  the  potential  collision  region  in  Figs.  5.5(a)  - 
(g).  The  rectangular  region  bounded  by  Sj  = 0,  sx  = 1,  s2  = 0,  and  s2  = 1 is  the 
valid  locations  of  potential  collisions  when  the  two  robots  move  in  the  prespecified 
line  segment  trajectories. 

When  the  potential  collision  region  intersects  with  the  border  s2  = 1 or  s,  = 
0 as  shown  in  Figs.  (5.5a)  and  (5.5d),  it  is  preferable  to  modify  the  trajectory  of 
robot  2.  In  contrast,  when  the  potential  collision  region  intersects  with  the  border 
sx  = 1 or  s2  = 0 as  shown  in  Figs.  (5.5b)  and  (5.5c),  it  is  preferable  to  modify  the 
trajectory  of  robot  1.  When  the  potential  collision  region  does  not  intersect  with 
the  borders  as  shown  in  Fig.  (5.5e),  both  robot  trajectories  can  be  modified.  Finally, 
when  the  potential  collision  region  intersects  with  two  opposite  sides  of  the  valid 
region  as  in  Figs.  (5.5f)  and  (5.5g),  collision  can  not  be  avoided  by  the  proposed 
methods. 

5.2.1  Time  Delay 

In  this  section,  it  is  assumed  that  the  trajectory  of  robot  2 is  to  be  modified 
using  time  delay  at  the  beginning  of  the  trajectory.  According  to  the  velocity  profile 
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of  robot  2 in  Fig.  5.2,  a distance  vs.  time  (s2-t)  curve,  which  represents  the 
displacement  profile  of  robot  2,  is  obtained  (see  Fig.  5.6).  In  addition,  the  potential 
collision  region  in  the  s2-Sj  plane  is  mapped  onto  the  s2-t  plane  a closed  region  as 
shown  in  Fig.  5.4.  In  order  for  robot  2 to  avoid  potential  collisions,  its  distance  vs. 
time  curve  must  lie  outside  the  potential  collision  region  in  the  s2-t  plane. 

The  time  delay  strategy  considered  here  is  to  delay  the  motion  of  robot  2 at 
the  beginning  point  of  its  path.  This  has  the  effect  of  shifting  the  distance  vs.  time 
curve  to  the  right. 

The  amount  of  time  that  the  motion  of  robot  2 must  be  delayed  can  be 
determined  as  follows.  Let  the  intersection  point  of  the  distance  vs.  time  curve  and 
the  potential  collision  region  in  the  s2-t  plane  be  denoted  by  M and  N.  The 
corresponding  traveled  distances  and  time  of  robot  2 at  these  two  points  are  denoted 
by  s2M,  s2N,  and  tM,  tN  (see  Fig.  5.7).  The  time  required  for  the  motion  of  robot  2 to 
be  delayed  is  determined  by  the  maximum  time  difference  between  the  distance  vs. 
time  curve  and  the  potential  collision  region  in  s2-t  plane  with  s2M  < s2  < s2N.  Fig.  5.8 
illustrates  the  result  of  the  distance  vs.  time  curve  of  robot  2 after  a time  delay. 

5.2.2  Speed  Reduction 

In  this  section,  another  approach  for  modifying  the  trajectories  of  robots  by 
using  the  concept  of  speed  reduction  is  discussed.  Speed  reduction  has  the  effect  of 
decreasing  the  slope  of  the  distance  versus  time  curve.  If  the  curve  is  modified  such 
that  it  is  outside  the  potential  collision  region,  no  collison  will  occur.  In  the  work 
of  Lee  and  Lee  [1987],  the  potential  collision  region  in  the  distance  vs.  time  domain 
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is  approximated  by  a bounding  box.  In  other  words,  the  potential  collision  region 
is  expanded  into  a box  to  simplify  the  problem. 

In  the  following,  the  potential  collision  region  is  expressed  explicitly,  and  is 
used  to  determine  the  location  of  collision  and  to  modify  the  trajectories  of  robots. 
The  method  applied  here  is  based  upon  the  assumption  that  a velocity  constraint  on 
the  end  effector  is  imposed.  Therefore,  the  slope  the  distance  vs.  time  curve  is 
limited  to  a value  which  is  denoted  by  vm.  This  suggests  that  two  lines  with  a slope 
equal  to  the  constrained  velocity  vm  and  being  tangent  to  the  potential  collision 
region  to  be  obtained.  Of  these  two  lines,  only  the  one  which  has  a smaller 
intercept  is  of  interest  and  is  denoted  by  /v.  This  line  together  with  a horizontal 
tangent  line  of  the  potential  collision  region,  i.e.,  sx  = s1>min  or  s2  = s2  min  is  used  to 
bound  the  potential  collision  region  (see  Fig.  5.9). 

In  the  following  discussion,  it  is  assumed  that  the  trajectory  of  robot  1 is  to 
be  modified  while  the  trajectory  of  the  other  robot  remains  unchanged.  The  line 
with  a slope  of  the  limiting  velocity  and  having  point  contact  with  the  potential 
collision  region  is  obtained  using  the  following  procedure.  First,  tangent  lines  of  the 
potential  collision  region  with  the  specified  slope  are  derived  for  each  time  interval. 
Among  these  tangent  lines,  the  one  with  a smallest  intercept  is  selected  as  the  line 
/v.  Let  the  line  /v  be  expressed  as 

Si  = s10  + vmt  (5.14) 

It  is  then  necessary  to  determine  the  value  of  s10. 

According  to  the  velocity  profile  of  robot  2,  the  distance  versus  time  curve  of 
robot  2 consists  of  segments  of  curves  of  two  different  types  which  can  be  expressed 
in  the  following  forms: 


108 


s2  = pt  + q 


(5.15a) 


and 


s2  = pt2  + qt  + r 


(5.15b) 


For  the  type  of  curve  in  Eq.  (5.15a),  the  mapped  potential  collision  region  in 
srt  plane  remains  a quadratic  form.  In  this  case,  the  tangent  line  with  slope  vm  is 
derived  first  by  replacing  st  in  the  boundary  equation  of  the  mapped  potential 
collision  region  by  Eq.  (5.14).  This  results  in  a second  order  polynomial  in  t.  If  a 
line  is  tangent  to  the  potential  collision  region,  a double  root  can  be  found  at  the 
point  contact.  In  other  words,  the  discriminant  of  the  second  order  equation  in  t 
must  equal  zero.  This  can  be  used  to  find  the  values  of  s10  such  that  the  line  in 
Eq.  (5.14)  is  tangent  to  the  potential  collision  region. 

When  the  displacement  profile  of  robot  2 is  in  the  form  of  Eq.  (5.15b),  the 
corresponding  potential  collision  region  in  s4-t  plane  becomes  a fourth  order  curve, 
Asx2  - 2BSj(pt2  + qt  + r)  + C(pt2  + qt  + r)2 


where 

A,  = A 

B0  = -2Bp,  Bl  = -2Bq,  B2  = -2(Br  - D) 

C0  = Cp2,  Cj  = 2Cpq,  C2  = C(q2  + 2pr)  - 2Ep, 
C3  = 2q(Cr  - E),  C4  = Cr2  - 2Er  + F 


+ 2Ds!  - 2E(pt2  + qt  + r)  + F = 0 


(5.16) 


or 
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For  each  value  of  t,  two  values  of  s,  can  be  computed.  The  lower  part  of  the 
potential  collision  region  is  derived  by  solving  Eq.  (5.17)  for  s,  and  is  expressed  as 
follows  (assume  Ag  > 0), 

si  = {-(B0t2  + B,t  + B2)  - [(B0t2  + B,t  + B2)2  - 

4A(i(C0t4  + C,t!  + Qt2  + C,t  + C,)],/2>  / (2A„)  (5.18) 

The  slope  of  the  above  curve  is  the  derivative  with  respect  to  time  and  is 
given  by 

ds,  / dt  = {-(2B0t  + B,)  - 0.5[[(B0t2  + Bjt  + B2)2  - 
4A0(C0t4  + C,t3  + C2t2  + C3t  + C4)]'1/2 
[2(B0t2  + B,t  + B2)(2Bgt  + B,) 

- 4Ag(4C0t3  + 3C,t2  + 2C2t  + C3)]}  / (2Ag)  (5.19) 

When  a line  with  slope  vm  is  tangent  to  the  curve  of  Eq.  (5.17),  the  slope  of 
the  curve  at  the  point  of  tangency  is  equal  to  the  slope  of  the  line.  This  suggests 
that  at  the  point  of  tangency,  the  following  function  must  be  satisfied: 

f(t)  = (ds,  / dt)  - vm  = 0 (5.20) 

which  can  be  used  to  solve  for  the  time  when  a point  contact  may  occur  between  the 
line  and  the  curve. 

From  the  above  discussion,  the  tangent  line  of  the  potential  collision  region 
at  each  time  interval  can  be  identified.  If  the  time  for  the  points  of  tangency  are 
outside  the  range  of  the  potential  collision  region  or  outside  the  time  interval  under 
consideration,  it  will  not  be  counted.  After  all  valid  tangent  lines  are  found,  the  one 
having  the  smallest  intercept  is  chosen  as  the  line  lv. 
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The  line  lv  intersects  with  the  line  sx  = slmin  in  a point  G.  It  is  proposed  to 
plan  the  motion  of  robot  1 such  that  its  distance  vs.  time  curve  passes  through  point 
G with  a velocity  as  high  as  possible. 

When  the  distance  vs.  time  curve  of  robot  1 passes  through  point  G,  a 
collision-free  motion  is  guaranteed.  This  is  because  robot  1 cannot  exceed  the 
limiting  velocity,  and  the  distance  vs.  time  curve  will  have  a slope  less  than  the 
limiting  velocity  and  will  not  intersect  the  potential  collision  region. 

The  trajectory  planning  of  robot  1 involves  the  planning  of  two  segments  of 
the  path.  The  first  segment  is  for  the  traveled  distance  s2  e [0,  sG]  and  it  occurs  at 
t e [t0,  tG].  The  second  segment  is  for  the  traveled  distance  sx  e [sG,  1]  and  it  occurs 
at  t e [tG,  tj.  It  is  necessary  to  have  a smooth  transition  at  t = tG. 

Suppose  the  trajectory  in  the  first  segment  involves  an  acceleration  part 
followed  by  a constant  velocity  part.  Denoted  by  taccl  the  time  for  the  acceleration 
part,  and  t^  the  time  for  the  constant  velocity  part.  It  follows  that 

taccl  ■*"  tyell  tG  (5-21) 


taccl  taccl  tyell 

I = sf 


(5.22) 

2|Y1-U1|  I&-UJ 

where  ^ is  the  magnitude  of  the  acceleration.  Solving  Eqs.  (5.21)  and  (5.22)  yields 

2s0 1 V,  • U, ! 


taccl  tG  i ( t( 


-) 


1/2 


(5.23) 


a, 


Since  taccl  must  be  less  than  or  equal  to  tG,  so 

2sg|V1-U1| 


taccl  tG  “ ( 


') 


1/2 


2 


(5.24) 


Ill 


and 

2sg  | Vj  - Uj  | 

W = tG  - taccl  = (tG2  - )1/2  (5.25) 

ai 

Note  that  if  taccl  = tG,  only  the  acceleration  part  is  contained  in  the  first 
segment  of  the  trajectory  and  the  velocity  vG  of  the  end  effector  at  tG  is  ajtaccl. 

In  order  to  determine  the  second  segment  of  the  trajectory,  it  is  assumed  that 
the  trajectory  contains  three  parts,  viz.,  acceleration,  constant  velocity,  and 
deceleration.  The  magnitude  of  acceleration  and  deceleration  is  a v The  time  of 
duration  in  each  part  is  denoted  by  tacc2,  t^,  and  tdec,  respectively.  The  robot  1 is 
accelerated  from  vG  to  vm  during  the  acceleration  part.  Then,  the  velocity  of  the  end 
effector  remains  constant  before  being  decelerated  to  a complete  stop  at  the  end 
point  Vj.  If  the  end  effector  reaches  the  velocity  vm  at  sx  = sG,  it  cannot  be 
accelerated  any  further.  In  this  case,  the  acceleration  part  in  the  second  segment  of 
the  trajectory  vanishes.  Otherwise,  we  have 

1 

1 = SG  [(VGb,cc2  adacc2  /2)  + (VG  + adacc2)^el2  + aiW  /2] 

IYi-UJ 

(5.26) 

Since  the  end  effector  is  accelerated  from  vG  to  vm,  thus  the  time  of  duration 
in  the  acceleration  part  is  given  by 
vm  - vG 

tacc2  = (5.27) 

ai 

Next,  the  end  effector  is  decelerated  from  vm  to  zero,  thus  the  time  of  duration  in 
the  deceleration  part  is  given  by 
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Idee 


(5.28) 


Substituting  Eqs.  (5.27)  and  (5.28)  into  Eq.  (5.26),  one  can  solve  for  t^. 
However,  if  t^  < 0,  the  trajectory  in  the  second  segment  contains  only  an 
acceleration  part  followed  by  a deceleration  part.  In  this  case,  the  time  of  duration 
tacc2  and  tdec  are  determined  by  the  following  equations: 


1 - sG  + 


IY.-U.I 


[(VGtacc2  aitacc2  /2)  + ai^dec  /2] 


and 


"t"  njtacc2  addec 

Substituting  tdec  in  (5.30)  into  (5.29)  yields 


(5.29) 


(5.30) 


aitacc2  2vGtacc2  + (Sq  - 1)  | Yx  - U4  | + 0 

2a1 


(5.31) 


The  above  quadratic  equation  can  be  used  to  solve  for  two  values  of  tacc2.  However, 
only  the  positive  one  is  of  interest  which  is  expressed  as  follows: 


1 

tac  = {-vG  + K2/2  + aj(l  - sG)  | - UJ]1/2} 

ai 

and  tdec  can  be  obtained  from  Eq.  (5.30). 


(5.32) 
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Fig.  5.1  Two  spheres  move  on  two  rectilinear  paths. 
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Fig.  5.2  Velocity  profiles  of  two  robots. 
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Fig.  5.3  Potential  collision  region  and  displacement  curve. 
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Fig.  5.4  Potential  collision  region  in  s2-t  plane. 
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(a)  (b) 


(c) 


(d) 


Fig.  5.5  Possible  locations  of  potential  collision  region,  (continued) 
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(e)  (f) 


(g) 


Fig.  5.5  Possible  locations  of  potential  collision  region. 


Fig.  5.6  Displacement  profile  of  robot  2. 


Fig.  5.7  Traveled  distance  vs.  time  curve  intersects  with 
the  potential  collision  region  at  M and  N. 
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Fig.  5.8  Displacement  curve  of  robot  2 after  a time  delay. 
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Potential  collision  region  is  bounded  by  tangent  lines. 


CHAPTER  6 

NUMERICAL  EXAMPLES 


The  purpose  of  this  chapter  is  to  demonstrate  the  development  in  the 
previous  chapters  by  numerical  examples  of  industrial  robots  such  as  the  Cincinnati 
Milacron  T3-776  and  PUMA  robots.  At  first,  the  permissible  orientations  of  robot 
links  are  demonstrated  with  the  end  effectors  of  the  robots  pinned  at  a target  point. 
It  is  assumed  that  the  joints  of  the  robots  have  no  angular  limitations.  Then 
examples  of  favorable  orientations  of  robot  links  are  displayed  with  the  end  effector 
pinned  at  a given  point  or  following  a prespecified  path  and  the  links  avoiding 
spherical  and  polyhedral  obstacles  in  the  work  environment.  This  is  followed  by 
demonstrations  of  favorable  orientations  of  a pair  of  robot  manipulators  moving  in 
coordination  within  a common  workspace.  Finally,  the  example  of  the  time 
scheduling  of  two  robot  manipulators  moving  on  a pair  of  rectilinear  paths  is 
demonstrated. 


6.1  Permissible  Orientations 

In  order  to  illustrate  the  permissible  orientations  of  robot  links,  a Cincinnati 
Milacron  T3-776  robot  and  a PUMA  robot  are  used  as  examples.  Skeletal  models 
of  the  robots  are  shown  in  Figs.  6.1  and  6.2,  and  the  dimensions  of  the  robots  are 
given  in  Tables  6.1  and  6.2. 
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The  permissible  orientation  ranges  of  the  upperarm  and  the  end  effector  of 
a T3-776  robot  when  the  end  effector  is  pinned  at  target  points  P(70",0",0")  and 
P(90",0",0")  are  shown  in  Figs.  6.3  and  6.4,  respectively.  They  describe  the  motion 
capability  of  the  robot  links  with  the  end  effector  pinned  at  P.  In  these  figures,  the 
abscissa  represents  the  locations  of  the  planes  of  operation  or  the  n planes  as 
described  in  Section  3.1,  and  the  ordinate  represents  the  orientations  of  robot  links 
with  respect  to  a horizontal  plane.  The  links  orientations  $2,  and  are  as 
defined  in  Section  3.2.1  and  are  measured  in  radian. 

In  Fig.  6.3(a),  the  permissible  orientations  of  the  upperarm  are  shown  to  lie 
within  two  nonintersecting  regions  which  represent  respectively  the  elbow  up  and 
elbow  down  configurations.  For  each  two  sectors  of  permissible  orientations  of 
the  upperarm  are  obtained.  Fig.  6.3(b)  shows  that  the  end  effector  is  capable  of  a 
complete  rotation  (0  - 2ir)  with  respect  to  the  target  point  P regardless  the  location 
of  the  7T  plane.  In  Fig.  6.4(a),  the  permissible  orientations  of  the  upperarm  are 
shown  to  lie  within  a symmetric  region  which  consists,  in  fact,  of  two  intersecting 
regions  representing  the  elbow  up  and  elbow  down  configurations,  respectively. 
Fig.  6.4(b)  shows  that  a void  exists  within  the  permissible  orientation  range  of  the 
end  effector.  When  the  orientations  of  the  end  effector  lie  within  this  void,  the 
wrist  joint  is  outside  the  constraint  surface  as  described  in  Section  3.1.  Therefore, 
they  are  nonpermissible. 

Analogously,  the  permissible  orientation  ranges  of  the  upperarm  and  the  end 
effector  of  a PUMA  robot  with  respect  to  target  points  P(25",0",0")  and  P(30",0",0") 
are  shown  in  Figs.  6.5  and  6.6,  respectively. 
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In  Fig.  6.5(a),  the  permissible  orientations  of  the  upperarm  are  shown  to  lie 
within  two  nonintersecting  regions  which  represent  respectively  the  elbow  up  and 
elbow  down  configurations.  Fig.  6.5(b)  shows  that  the  end  effector  is  capable  of  a 
complete  rotation  (0  - 2n)  with  respect  to  the  target  point  P regardless  the  location 
of  the  7r  plane.  In  Fig.  6.6(a),  the  permissible  orientations  of  the  upperarm  are 
shown  to  lie  within  a region  which  consists  of  two  intersecting  regions  representing 
the  elbow  up  and  elbow  down  configurations,  respectively.  Fig.  6.6(b)  shows  that 
a void  exists  within  the  permissible  orientation  range  of  the  end  effector.  When  the 
orientations  of  the  end  effector  lie  within  this  void,  the  wrist  joint  is  outside  the 
constraint  surface.  Therefore,  they  are  nonpermissible. 

6.2  Spherical  Obstacles 

In  this  section,  favorable  orientation  ranges  of  robot  links  are  demonstrated 
which  direct  the  links  avoiding  collision  with  a spherical  obstacle.  At  first,  the 
favorable  orientations  of  robot  links  are  shown  when  the  end  effector  link  is  pinned 
at  a specific  point.  In  this  case,  the  favorable  orientation  ranges  are  determined  for 
various  n planes  where  the  upperarm  and  forearm  lie. 

Then,  the  favorable  orientations  of  the  robot  links  when  the  end  effector 
follows  a prespecified  path  are  demonstrated.  In  this  case,  two  approaches  for  the 
determination  of  the  favorable  orientations  are  used.  The  first  approach  is  to 
determine  the  favorable  orientation  range  for  each  point  on  the  path  by  using  the 
7r  plane  which  contains  the  forearm  and  the  end  effector  of  the  robot.  In  contrast, 
the  second  approach  is  to  determine  the  favorable  orientation  range  for  each  point 
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on  the  path  by  using  the  n plane  where  the  upperarm  has  a maximum  orientation 
ranges. 

The  permissible  orientation  range  of  the  upperarm  of  a T3-776  robot  with 
respect  to  a target  point  P(60",30",4")  in  a world  coordinate  system  is  shown  in  Fig. 
6.7,  which  describes  the  motion  capability  of  the  robot  link  when  the  end  effector 
is  pinned  at  P.  The  world  coordinate  system  is  located  at  the  intersection  of  axes 
of  the  base  joint  and  shoulder  joint  of  the  robot.  Here,  the  robot  is  assumed 
maintaining  an  elbow  up  and  wrist  up  configuration.  In  this  figure,  the  abscissa 
is  the  angle  between  a ir  plane  and  a vertical  plane  containing  the  z-axis  and  the 
target  point  P.  When  = 0,  i.e.,  the  n plane  passes  through  the  point  P,  the 
upperarm  has  a maximum  permissible  orientation  range.  Fig.  6.8  shows  the 
favorable  orientation  ranges  of  the  upperarm  with  respect  to  a spherical  obstacle 
with  center  (60", 20", 30")  and  radius  10".  This  demonstrates  the  capability  of  the 
robot  links  to  move  without  colliding  with  the  spherical  obstacle.  It  can  be  seen  that 
there  is  a void  within  the  favorable  orientation  ranges.  This  implies  that  when  the 
orientation  of  the  upperarm  is  inside  this  void,  the  robot  links  will  interfere  with  the 
spherical  obstacle. 

Next,  the  end  effector  of  the  robot  is  to  move  along  a rectilinear  path  which 
can  be  expressed  in  the  parametric  form  as  follows 

P = U + s(Y  - U)  0 < s < 1 (6.1) 

where  U and  V are  the  starting  and  final  points,  respectively,  and  their  coordinates 
are  (85",0",10")  and  (60", 30", 4")  in  the  world  coordinate  system.  In  addition,  the 
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robot  must  avoid  a spherical  obstacle  with  center  (60", 20", 30")  and  radius  10".  For 
each  point  on  the  path,  an  analysis  of  the  favorable  orientations  of  the  robot  links 
can  be  performed  according  to  the  relative  location  of  the  robot  and  the  obstacle. 
This  analysis  is  repeated  for  a sequence  of  points  on  the  path  and  any  orientation 
within  the  favorable  orientation  range  can  be  used  to  direct  robot  links  to  avoid 
interference.  Fig.  6.9  illustrates  the  favorable  orientation  range  of  the  robot  links 
along  the  straight  line  path.  In  this  example,  the  links  of  the  robot  remain  on  the 
same  plane  throughout  the  motion.  In  other  words,  the  robot  is  moving  as  a planar 
3R  robot  rotating  about  the  first  vertical  axis.  In  the  figure,  the  abscissa  s is  the 
displacement  parameter  along  the  path  and  the  ordinate  $2  is  the  orientations  of  the 
upperarm.  It  can  be  seen  that  the  favorable  orientations  of  the  upperarm  lie  within 
the  upper  and  lower  curves  which  are  essentially  the  same  as  the  permissible 
orientations.  However,  there  is  a void  in  the  figure  of  favorable  orientation  ranges, 
which  means  that  the  links  of  the  robot  will  interfere  with  the  spherical  obstacle 
when  the  orientation  of  the  upperarm  is  inside  the  void. 

In  contrast,  Fig.  6.10  illustrates  the  optimum  favorable  orientation  range  of 
the  robot  links  along  the  straight  line  path.  The  optimum  favorable  orientation 
range  is  obtained  by  computing  the  favorable  orientations  for  a specific  target  point. 
The  location  of  the  n plane  where  the  upperarm  has  a maximum  favorable 
orientation  range  is  identified  and  the  corresponding  favorable  orientation  range  is 
said  to  be  optimum.  This  procedure  is  repeated  for  a sequence  of  target  points 
along  the  path.  Fig.  6.11  shows  the  locations  of  n planes  where  the  upperarm  has 
a maximum  favorable  orientation  range.  The  locations  of  tt  planes  are  illustrated 
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by  the  ordinate  <J>,  which  represents  the  angle  between  the  n plane  and  the  x-z  plane 
of  the  world  coordinate  system. 

Similarly,  the  permissible  orientation  range  of  the  upperarm  of  a PUMA 
robot  with  respect  to  a target  point  P(20",5",0")  in  a world  coordinate  system  is 
shown  in  Fig.  6.12.  The  world  coordinate  system  is  located  at  the  intersection  of  the 
axes  of  the  base  joint  and  the  shoulder  joint  of  the  robot.  In  this  figure,  the  abscissa 
is  the  angle  between  a n plane  and  a vertical  plane  containing  the  z-axis  and  the 
target  point  P.  At  -iq  = 0,  the  n plane  passes  through  the  point  P.  Here,  the  robot 
is  assumed  maintaining  an  elbow  up  and  wrist  up  configuration.  Fig.  6.13  shows  the 
favorable  orientation  range  with  respect  to  a spherical  obstacle  with  center 
(18",10",0")  and  radius  5".  It  can  be  seen  that  no  favorable  orientation  exists  when 
is  greater  than  0.1  and  part  of  permissible  orientations  are  not  favorable  when 
is  between  -0.06  and  0.1. 

Next,  the  end  effector  of  a robot  is  to  move  along  a rectilinear  path  which  is 
expressed  in  the  form  of  Eq.  (6.1).  The  coordinates  of  the  starting  and  final  points 
of  the  path  are  given  by  U(30",-5",-l")  and  V(25",10",-5"),  respectively.  In  addition, 
the  robot  must  avoid  a spherical  obstacle  with  center  (34",0",13")  and  radius  11".  Fig. 
6.14  illustrates  the  favorable  orientation  range  of  the  robot  links  along  the  straight 
line  path.  In  this  example,  the  forearm  and  the  end  effector  of  the  robot  remain  on 
the  same  plane  throughout  the  motion.  In  the  figure,  the  abscissa  s is  the 
displacement  parameter  along  the  path  and  the  ordinate  $2  is  the  orientations  of  the 
upperarm.  The  favorable  orientations  of  the  upperarm  lie  between  the  upper  and 
lower  curves  and  when  s is  between  0 and  0.4,  the  favorable  orientation  range  is 
small  due  to  the  presence  of  the  spherical  obstacle. 
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In  contrast,  Fig.  6.15  illustrates  the  optimum  favorable  orientation  range  of 
the  robot  links  along  the  straight  line  path.  The  optimum  favorable  orientation 
range  is  obtained  by  computing  the  favorable  orientations  for  a specific  target  point. 
The  location  of  the  n plane  where  the  upperarm  has  a maximum  favorable 
orientation  range  is  identified  and  the  corresponding  favorable  orientation  range  is 
said  to  be  optimum.  This  procedure  is  repeated  for  a sequence  of  target  points 
along  the  path.  Fig.  6.16  shows  the  locations  of  n planes  where  the  upperarm  has 
a maximum  favorable  orientation  range.  The  locations  of  n planes  are  determined 
by  the  values  in  the  ordinate  which  represents  the  angle  between  the  ir  plane  and 
the  x-z  plane  of  the  world  coordinate  system. 

6.3  Polyhedral  Obstacles 

In  this  section,  favorable  orientation  ranges  of  robot  links  are  determined 
which  direct  the  links  avoiding  collision  with  a polyhedral  obstacle.  At  first,  the 
favorable  orientations  of  robot  links  are  shown  when  the  end  effector  link  is  pinned 
at  a specific  point.  Then,  the  favorable  orientations  of  the  robot  links  when  the  end 
effector  follows  a prespecified  path  are  demonstrated. 

The  permissible  orientation  range  of  the  upperarm  of  a T3-776  robot  with 
respect  to  a target  point  P(80",40",0")  in  a world  coordinate  system  is  derived  and  is 
shown  in  Fig.  6.17,  which  describes  the  motion  capability  of  the  robot  link  when  the 
end  effector  is  pinned  at  P.  In  this  figure,  the  abscissa  is  the  angle  between  the 
n plane  and  a vertical  plane  containing  the  z-axis  and  the  target  point  P.  The 
maximum  and  minimum  permissible  orientations  are  represented  by  the  upper  and 
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lower  curves  in  the  figure.  Here,  the  robot  is  assumed  maintaining  an  elbow  up  and 
wrist  up  configuration.  Fig.  6.18  displays  the  favorable  orientation  ranges  which 
describe  the  capability  of  the  robot  links  to  move  without  interfering  a polyhedral 
obstacle  whose  vertices  are  A(75",30”,30”),  B(75M,15",30"),  C(  112",  15", 30"), 
D(  112", 30", 30"),  E(75",30",12"),  F(75",15",12"),  G(  112",  15",  12"),  and  H(  112", 30",  12"). 
It  can  be  seen  that  the  favorable  orientations  of  the  upperarm  lie  within  the  upper 
and  lower  curves  which  are  essentially  the  same  as  the  permissible  orientations. 
However,  there  is  a void  in  the  figure  of  favorable  orientation  ranges,  which  means 
that  the  links  of  the  robot  will  interfere  with  the  polyhedral  obstacle  when  the 
orientation  of  the  upperarm  is  inside  the  void. 

Further,  the  end  effector  of  the  robot  is  assumed  moving  along  a rectilinear 
path  which  is  expressed  in  the  form  of  Eq.  (6.1).  The  coordinates  of  the  starting 
and  final  points  of  the  path  are  given  by  U(85",0",10")  and  V(60",30",4"),  respectively. 
In  addition,  the  robot  must  avoid  a polyhedral  obstacle  with  vertices  A(75",30",30"), 
B(75",15",30"),  C(  112",  15", 30"),  D(  112", 30", 30"),  E(75",30",12"),  F(75",15",12"), 
G(  112",  15",  12"),  and  H(  112", 30",  12"). 

For  each  point  on  the  path,  an  analysis  of  the  favorable  orientations  of  the 
robot  links  can  be  performed  according  to  the  relative  location  of  the  robot  and  the 
obstacle.  This  analysis  is  repeated  for  a sequence  of  discrete  points  on  the  path  and 
any  orientation  within  the  favorable  orientation  range  can  be  used  to  direct  the 
robot  links  to  avoid  interference.  The  favorable  orientation  range  of  the  robot  is 
shown  in  Fig.  6.19.  The  locations  of  the  n planes  for  this  derivation  are  obtained 
by  using  a linear  interpolation  of  the  locations  of  the  n planes  at  the  starting  and 
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final  points  of  the  path  which  are  assumed  given.  In  the  figure,  the  abscissa  s is  the 
displacement  parameter  along  the  path  and  the  ordinate  $2  is  the  orientations  of  the 
upperarm.  It  can  be  seen  that  there  is  a void  in  the  figure  of  favorable  orientation 
ranges,  which  means  that  the  links  of  the  robot  will  interfere  with  the  polyhedral 
obstacle  when  the  orientation  of  the  upperarm  is  inside  the  void.  Fig.  6.20  shows 
the  locations  of  the  x planes  along  the  path.  In  this  figure,  is  the  angle  between 
the  selected  7r  plane  and  the  x-z  plane  of  the  world  coordinate  system,  and  is  the 
angle  between  the  selected  n plane  and  the  vertical  plane  which  passes  through  the 
z-axis  and  the  target  point.  The  curve  is  linear  due  to  the  use  of  the  linear 
interpolation  approach. 

Similarly,  the  permissible  orientation  range  of  the  upperarm  of  a PUMA 
robot  with  respect  to  a target  point  P(30",9",-4")  in  a world  coordinate  system  is 
derived  and  is  shown  in  Fig.  6.21,  which  describes  the  motion  capability  of  the  robot 
link  when  the  end  effector  is  pinned  at  P.  In  this  figure,  the  abscissa  is  the  angle 
between  the  7r  plane  and  a vertical  plane  containing  the  z-axis  and  the  target  point 
P.  Here,  the  robot  is  assumed  maintaining  an  elbow  up  and  wrist  up  configuration. 
Fig.  6.22  shows  the  favorable  orientation  range  which  direct  the  robot  links  to  move 
without  interfering  a polyhedral  obstacle  whose  vertices  are  A(20",5",-l"),  B(34",-5",- 
1"),  C(39",2",-l"),  D(27",10M,-r),  E(20",5",-12"),  F(34",-5",-12"),  G(39",2",-12"),  and 
H(27",10",-12").  Comparing  the  favorable  orientation  ranges  with  the  permissible 
orientation  ranges,  we  can  find  that,  for  the  orientations  of  between  -0.1  and  - 
0.18,  part  of  the  permissible  orientations  are  not  favorable  due  to  the  presence  of 
the  polyhedral  obstacle. 
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In  the  following  example,  the  end  effector  of  a solid-link-modeled  PUMA 
robot  is  to  move  along  a rectilinear  path  which  is  expressed  in  the  form  of  Eq.  (6.1). 
The  coordinates  of  the  starting  and  final  points  of  the  path  are  given  by  U(32",-5",- 
3")  and  V(24”,22",-4"),  respectively.  In  addition,  the  robot  must  avoid  a polyhedral 
obstacle  with  vertices  A(18",5,,,-l"),  B(22",-5,,,-l"),  C(25",-2",-l"),  D(22n,7,,,-1"), 
E(18",5",-52"),  F(22",-5",-52"),  G(25”,-2",-52"),  and  H(22,,,7",-52M).  The  robot  is 
assumed  moving  in  an  elbow  up  and  wrist  down  configuration. 

Fig.  6.23  shows  the  favorable  orientation  ranges  of  the  robot  links.  The 
locations  of  the  n planes  for  this  derivation  are  obtained  by  using  a linear 
interpolation  of  the  locations  of  the  7r  planes  at  the  starting  and  final  points  of  the 
path.  In  the  figure,  the  abscissa  s is  the  displacement  parameter  along  the  path  and 
the  ordinate  $2  is  the  orientations  of  the  upperarm.  When  the  orientations  of  the 
upperarm  lie  between  the  upper  and  lower  curves  of  the  figure,  the  links  of  the 
robot  will  not  interfere  with  the  polyhedral  obstacle.  Fig.  6.24  shows  the  locations 
of  the  7r  planes  along  the  path.  In  the  figure,  is  the  angle  between  the  selected 
7r  plane  and  the  x-z  plane  of  the  world  coordinate  system,  and  a$i  is  the  angle 
between  the  selected  7r  plane  and  the  vertical  plane  which  passes  through  the  z-axis 
and  the  target  point. 

6,4  Coordination  of  Two  Robot  Manipulators 

This  section  is  devoted  to  the  coordinations  of  two  robot  manipulators  under 
parallel  operations  and  master-slave  operations.  For  the  parallel  operations,  an 
example  of  coordination  of  two  T3-776  robots  modeled  by  line  segments  is  presented. 
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At  first,  the  permissible  orientation  ranges  of  each  robot  with  the  end  effector 
following  a prespecified  trajectory  are  shown.  Further,  favorable  orientation  ranges 
which  are  obtained  using  the  method  described  in  Section  3.4  are  illustrated. 
Finally,  sets  of  joint  angle  displacements  for  the  two  robots  throughout  the  trajectory 
are  displayed.  Similar  results  are  also  shown  for  two  PUMA  robots  with  solid  links. 
In  addition,  computer  graphics  animation  of  two  PUMA  robots  are  used  to  verify  the 
robots’  motions. 

For  the  master-slave  operations,  an  example  of  coordination  of  two  PUMA 
robots  with  solid  links  is  presented. 

6.4.1  Parallel  Operations 

Suppose  the  end  effectors  of  two  T3-776  robots  are  moving  along  rectilinear 
trajectories  as  in  Eq.  (6.1)  with  starting  points  ^(36",  66.5",  30"),  U2(47",40",10")  and 
final  points  V1(72",57.5",10"),  V2(38",77",20"),  respectively,  in  a world  coordinate 
system.  The  world  coordinate  system  is  located  at  the  intersection  of  the  axes  of 
the  base  joint  and  the  shoulder  joint  of  the  first  robot,  and  the  coordinates  of  the 
shoulder  joint  of  the  second  robot  in  this  coordinate  system  are  (104",  108",  0").  The 
end  effectors  of  two  robots  are  assumed  moving  with  an  acceleration  followed  by  a 
deceleration  and  they  arrive  the  goal  point  at  the  same  time.  The  displacements  of 
the  end  effectors  of  the  two  robots  are  expressed  in  terms  of  the  parameters  st  and 
s2  as  follows: 

Si  = 


(1)  t0  < t < tt 


2 1 Yi  - Uj  | 


(i  = 1,  2) 


(6.2) 


132 


ai(tf  - t) 

(2)  tx  < t < tf  S;  = 1 (6.3) 

2 1 Vj  - U,  | 

It  is  assumed  that  the  links  of  the  robots  remain  on  the  same  plane  during  the 
motion.  Figs.  6.25  and  6.26  show  the  permissible  orientation  ranges  of  the  two 
robots  along  their  trajectories  when  they  are  in  elbow  up  and  wrist  up  configurations. 
In  the  figures,  the  abscissas  are  the  displacement  parameters  along  the  trajectories 
and  the  ordinates  are  the  orientations  of  the  upperarms  of  the  two  robots.  For  each 
point  in  the  trajectory  of  a robot,  a range  of  permissible  orientations  can  be  found. 
The  favorable  orientation  ranges  of  these  two  robots  which  avoid  interference 
between  the  two  robots  are  computed  and  are  displayed  in  Figs.  6.27  and  6.28.  It 
can  be  seen  that  when  sx  is  between  0.3  and  0.5  and  s2  is  between  0.25  and  0.5,  parts 
of  permissible  orientations  are  not  favorable,  which  means  that  in  those  orientations 
the  two  robots  may  interfere  with  each  other.  Also  shown  in  the  figures  are 
orientations  of  the  two  upperarms  that  can  be  selected  for  interference-free  motion 
of  the  two  robots. 

Analogously,  Figs.  6.29  and  6.30  show  the  permissible  orientation  ranges  of 
two  solid-modeled  PUMA  robots  moving  along  rectilinear  trajectories  with  starting 
points  U1(26",20",-3"),  U2(31",-l",3")  and  final  points  V1(32",5",-3"),  V2(28",23,6"), 
respectively,  in  a world  coordinate  system  which  is  located  at  the  intersection  of  the 
axes  of  the  base  joint  and  shoulder  joint  of  the  first  robot,  and  the  coordinates  of  the 
shoulder  joint  of  the  second  robot  in  this  coordinate  system  are  (60",  0",  0").  The 
displacements  of  the  end  effectors  of  the  robots  are  represented  by  Eqs.  (6.2)  and 
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(6.3).  In  this  example,  the  forearm  and  end  effector  of  the  robots  are  assumed 
remaining  on  the  same  plane  throughout  the  motion. 

The  favorable  orientation  ranges  of  these  two  robots  which  avoid  interference 
between  the  two  robots  are  computed  and  are  shown  in  Figs.  6.31  and  6.32.  It  can 
be  seen  the  two  robots  may  interfere  with  each  other  when  sx  is  between  0.6  and  0.8 
and  s2  is  between  0.5  and  0.7.  Finally,  sets  of  joint  angle  displacements  of  the  two 
robots  which  lie  within  the  favorable  orientation  range  are  illustrated  in  Figs.  6.33 
and  6.34.  The  resulting  motion  of  the  two  robots  were  then  verified  by  using 
computer  graphics  animation  in  a Silicon  Graphics  IRIS  4D/70GT  Workstation  as 
shown  in  Fig.  6.35. 

6.4.2  Master-Slave  Operations 

In  this  section,  an  example  of  coordination  of  two  solid-modeled  PUMA 
robots  operating  in  a master-slave  manner  is  demonstrated.  At  first,  the  motion  of 
the  master  robot  is  determined  such  that  it  follows  a desired  trajectory.  It  is 
assumed  that  the  end  effector  of  the  master  robot  moves  in  a rectilinear  path  with 
starting  point  U1(26",20",0")  and  final  point  V1(32",5",0")  with  a constant  orientation 
^(0,0,1)  and  S7(  0,1,0)  in  a world  coordinate  system  which  is  located  at  the 
intersection  of  the  axes  of  the  base  joint  and  the  shoulder  joint  of  the  first  robot, 
and  the  coordinates  of  the  shoulder  joint  of  the  second  robot  in  this  coordinate 
system  are  (62",  0",  0").  A set  of  joint  angles  can  then  be  obtained  by  a reverse 
analysis  and  is  shown  in  Fig.  6.36.  Here,  the  end  effector  of  the  robot  is  assumed 
moving  with  a constant  velocity. 
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The  slave  robot  is  to  follow  a rectilinear  path  with  starting  point  U2(33",-l",- 
6")  and  final  point  V2(30",20",-8")  with  a constant  velocity.  At  each  time  instant,  the 
target  point  of  the  slave  robot  is  known  and  the  favorable  orientation  ranges  of  the 
slave  robot  are  computed  such  that  the  links  of  the  slave  robot  do  not  interfere  with 
the  links  of  the  master  robot.  Fig.  6.37  shows  the  favorable  orientation  ranges  of  the 
upperarm  of  the  slave  robot  along  the  path.  In  this  figure,  the  slave  robot  is  in 
elbow  up  and  wrist  up  configuration.  It  can  be  seen  that  when  s2  is  between  0.45 
and  0.7,  the  links  of  the  slave  robot  may  interfere  with  the  master  robot,  and  the 
favorable  orientation  ranges  in  this  segment  of  trajectory  are  samller  than  the 
permissible  orientation  ranges.  Also  shown  in  the  figure  are  orientations  of  the 
upperarm  of  the  slave  robot  that  can  be  selected  to  avoid  interference  with  the 
master  robot.  A set  of  joint  angles  of  the  slave  robot  which  lie  within  the  favorable 
orientation  ranges  are  illustrated  in  Fig.  6.38.  The  resulting  motion  of  the  two 
robots  were  then  verified  by  using  computer  graphics  animation  in  a Silicon  Graphics 
IRIS  4D/70GT  Workstation  as  shown  in  Fig.  6.39. 

6.5  Time  Scheduling  of  Two  Robot  Manipulators 

Consider  that  the  end  effectors  of  two  robot  manipulators  move  along  straight 
line  paths  with  beginning  points  17",  17.5", 0"),  U2(1.5",45",0")  and  final  points 
V1(87",37",0"),  V2(75",22",0"),  respectively.  The  sum  of  link  lengths  of  these  two  end 
effectors  is  15".  Using  this  information,  the  potential  collision  region  of  two  robot 
manipulators  moving  in  the  above  straight  line  paths  is  determined  by  Eq.  5.5  and 
is  illustrated  in  Fig.  6.40. 
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Further,  the  velocity  profile  of  the  robot  manipulators  are  given  in  Fig.  6.41. 
Robot  1 accelerates  from  t = 0 to  t = 3 sec  with  magnitude  of  acceleration  ^ = 2.7 
in/sec2,  followed  by  a constant  velocity  until  t = 9 sec  and  finally  decelerates  with 
magnitude  -2.7  in/sec2  until  a complete  stop  at  point  Wv  Robot  2 has  a similar 
velocity  profile  but  the  magnitudes  of  acceleration  and  deceleration  are  a2  = 2.4 
in/sec2  and  -2.4  in/sec2,  the  maximum  velocity  is  9.6  in/sec,  and  the  time  of 
transition  are  t = 4 sec  and  t = 8 sec.  The  total  distances  traveled  for  the  two 
robots  are  73"  and  77",  respectively. 

Using  the  velocity  profile,  the  corresponding  positions  of  the  two  robots 
throughout  the  motion  are  obtained  and  are  described  in  the  parametric  space  as 
shown  in  Fig.  6.42.  Also  shown  in  the  figure  is  the  potential  collision  region.  It  can 
be  seen  that  the  curve  of  corresponding  positions  of  two  robots  intersects  with  the 
potential  collision  region,  which  implied  that  the  two  robots  will  interfere  with  each 
other  while  tracking  the  specified  trajectories.  Further,  the  positions  where  the 
interference  starts  and  ends  are  respectively  st  = 0.503,  s2  = 0.504  and  st  = 0.876, 
s2  = 0.896.  Since  the  potential  collision  region  in  the  parametric  space  intersects 
with  the  line  s2  = 1,  it  is  therefore  desirable  to  modify  the  trajectory  of  robot  2 to 
avoid  interference. 

In  order  to  modify  the  trajectory  of  robot  2,  the  potential  collision  region  is 
mapped  into  a space-time  (s2-t)  domain  as  shown  in  Fig.  6.43.  The  traveled  distance 
versus  time  curve  for  the  original  trajectory  of  robot  2 is  also  displayed  in  the  figure. 
The  time  when  the  interference  starts  and  ends  are  computed  as  6.0  sec  and  9.4  sec, 
respectively. 
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The  time  difference  between  the  traveled  distance  versus  time  curve  and  the 
potential  collision  region  in  the  interval  of  s2  = [0.504,  0.896]  is  shown  in  Fig.  6.44. 
The  maximum  time  difference  is  0.43  sec,  which  is  used  as  a time  delay  in  the 
starting  point  of  robot  2.  The  resulting  trajectory  can  be  realized  by  the  traveled 
distance  versus  time  curve  in  Fig.  6.45.  The  curve  is  shifted  to  the  right  by  an 
amount  of  dtmax  ( = 0.43  sec),  thus  it  moves  out  of  the  potential  collision  region  and 
no  interference  will  occur.  The  total  travel  time  of  the  new  trajectory  is  12.43  sec. 

Another  method  of  modifying  the  trajectory  of  robot  2 is  to  reduce  the  speed 
of  the  robot  to  avoid  the  potential  collision  region.  First,  a transition  point  G 
outside  the  potential  collision  region  in  the  space-time  domain  is  located.  The 
transition  point  G is  the  intersection  point  of  two  lines  (see  Fig.  6.46).  One  line  has 
a slope  of  a limiting  velocity  vm  (=10  in/sec)  and  has  a point  contact  with  the 
potential  collision  region.  The  other  line  is  horizontal  and  is  tangent  to  the  potential 
collision  region  at  the  point  with  minimum  s2.  The  new  trajectory  is  to  pass  through 
the  point  G.  The  traveled  distance  versus  time  curve  of  the  new  trajectory  is  shown 
in  Fig.  6.47.  The  curve  consists  of  five  segments,  i.e.,  acceleration,  constant  velocity, 
acceleration,  constant  velocity  and  deceleration,  and  can  be  expressed  in  five  time 
intervals  as  follows: 

(1)  0 < t < taccl 


2 1 V2  - H2 1 
(2)  taccl  < t < tG 


(6.4) 
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s2  - s2a  + 


^2^accl(l  ” heel) 


(6.5) 


where 


^2^accl 

2 1 V2  - U2 1 


(3)  tG  < t < tb 

a2tacci(t  * to)  a2(t  - tG)2 

s2  = sG  + + 

|V2-U2|  2 1 V2  - U2 1 

(4)  tb  < t < tc 

vm(t  - to) 

s2  = s2b  + 

iy2-u2i 

where 

^2laccl(^b  " 1g)  a2(h>  ' hi) 

$2b  = Sq  + + 

|v2-u2|  2 1 y2  - U2 1 


(6.6) 


(6.7) 


(5)  tc  < t < td 

a2(td  - t)2 

s2  = 1 (6.8) 

2 1 Y2  - U2 1 

where  taccl  = 2.95  sec,  tG  = 5.53  sec,  tb  = 6.74  sec,  tc  = 8.44  sec,  and  td  = 12.60  sec. 


The  total  travel  time  of  the  new  trajectory  after  a speed  reduction  is  12.60  sec. 


138 


Fig.  6.1  Skeletal  model  of  a Cincinnati  Milacron  T3-776  robot. 


Link 
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Value 

(in) 

Offset 

Value 

(in) 

Twist 

Angle 

Value 

(deg) 

ai2 

0 

Su 

a 12 

90 

a23 

44 

S22 

0 

^23 

0 

a34 

0 

S33 

0 

a34 

90 

a45 

0 

S44 

55 

a45 

61 

a56 

0 

S55 

0 

<*56 

61 

a67 

0 

s«* 

15 

a61 

Table  6.1  Dimensions  of  Cincinnati  Miracron  T3-776  robot. 
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Fig.  6.2  Kinematic  model  of  a PUMA  robot. 


Link 

Length 

Value 

(in) 

Offset 

Value 

(in) 

Twist 

Angle 

Value 

(deg) 

ai2 

0 

Su 

“12 

270 

a23 

17 

S22 

9.545 

a23 

0 

a34 

0 

S33 

3.675 

a34 

270 

a45 

0 

S44 

17.05 

a45 

90 

a56 

0 

S55 

0 

“56 

270 

a67 

0 

S66 

6 

abl 

90 

Table  6.2  Dimensions  of  PUMA  robot. 
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Fig.  6.3  Permissible  orientations,  target  point  P = (70",0",0") 
(a)  upperarm,  (b)  end  effector 
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Fig.  6.4  Permissible  orientations,  target  point  P = (90",0",0") 
(a)  upperarm,  (b)  end  effector 
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Fig.  6.5  Permissible  orientations,  target  point  P = (25",0",0") 
(a)  upperarm,  (b)  end  effector 
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Fig.  6.6  Permissible  orientations,  target  point  P = (30",0",0") 
(a)  upperarm,  (b)  end  effector 
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Fig.  6.7  Permissible  orientation  ranges  of  a T3-776  robot. 
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Fig.  6.8  Favorable  orientation  ranges  of  a T3-776  robot. 
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Fig.  6.9  Favorable  orientation  range  along  a straight  line  path. 
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Fig.  6.10  Optimum  favorable  orientation  range  along  a straight  line  path. 


Fig.  6.11  Optimum  location  of  n plane  ($x)  along  a straight  line  path. 
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Fig.  6.12  Permissible  orientation  ranges  of  a PUMA  robot. 
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Fig.  6.13  Favorable  orientation  ranges  of  a PUMA  robot. 
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Fig.  6.14  Favorable  orientation  range  along  a straight  line  path. 
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Fig.  6.15  Optimum  favorable  orientation  range  along  a straight  line  path. 
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Fig.  6.16  Optimum  location  of  n plane  ($t)  along  a straight  line  path. 
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Fig.  6.17  Permissible  orientation  ranges  of  a T3-776  robot. 


favorable  orientation  range 


-0.2  -0.15  -0.1  -0.05  0 0.05  0.1  0.15  0.2 


phi  1 


Fig.  6.18  Favorable  orientation  ranges  of  a T3-776  robot. 
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Fig.  6.19  Favorable  orientation  ranges  along  a straight  line  path. 
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Fig.  6.20  Orientations  of  selected  n planes  along  the  path. 
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Fig.  6.21  Permissible  orientation  ranges  of  a PUMA  robot. 
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Fig.  6.22  Favorable  orientation  ranges  of  a PUMA  robot. 
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Fig.  6.23  Favorable  orientation  ranges  along  a straight  line. 
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Fig.  6.24  Orientations  of  selected  n planes  along  the  path. 
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Fig.  6.25  Permissible  orientation  ranges  of  robot  1. 
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Fig.  6.26  Permissible  orientation  ranges  of  robot  2. 
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Fig.  6.27  Favorable  orientation  ranges  of  robot  1. 
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Fig.  6.28  Favorable  orientation  ranges  of  robot  2. 
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Fig.  6.29  Permissible  orientations  ranges  of  robot  1. 
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Fig.  6.30  Permissible  orientation  ranges  of  robot  2. 
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Fig.  6.31  Favorable  orientation  ranges  of  robot  1. 
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Fig.  6.32  Favorable  orientation  range  of  robot  2. 
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Fig.  6.33  Joint  angles  of  robot  1. 
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Fig.  6.34  Joint  angles  of  robot  2. 
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Fig.  6.35  Simulation  of  robots’  motions. 
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Fig.  6.36  Joint  angles  of  the  master  robot. 
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Fig.  6.37  Favorable  orientation  ranges  of  the  slave  robot. 
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Fig.  6.38  Joint  angles  of  the  slave  robot. 


Fig.  6.39  Simulation  of  robots’  motions. 
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Fig.  6.40  Potential  collision  region. 
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Fig.  6.41  Velocity  profiles  of  two  robots. 
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Fig.  6.42  Distances  curve  in  parametric  space. 
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Fig.  6.43  Distance  vs.  time  curve  of  robot  2. 
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Fig.  6.44  Time  difference  between  the  distance  vs.  time  curve 
and  the  potential  collision  region 
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Fig.  6.45  Distance  vs.  time  curve  after  a time  delay. 
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Fig.  6.46  Transition  point  G. 
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Fig.  6.47  Distance  vs.  time  curve  after  a speed  reduction. 


CHAPTER  7 

CONCLUSIONS  AND  RECOMMENDATIONS 


In  this  research,  the  determination  of  the  capability  of  a robot  manipulator 
to  follow  a prespecified  path  while  avoiding  obstacles  and  a second  robot 
manipulator  has  been  demonstrated.  This  analysis  considers  first  the  motion 
capabilities  of  the  links  of  a robot  manipulator  while  its  end  effector  is  pinned  at  a 
target  point.  Then,  the  motion  capabilities  of  the  links  of  a robot  manipulator  while 
its  end  effector  follows  a prespecified  path  are  determined.  Permissible  orientation 
ranges,  allowable  orientation  ranges  and  favorable  orientation  ranges  are  used  to 
describe  the  capabilities  of  the  links  to  avoid  interference.  The  joint  angles  of  the 
robot  links  are  then  chosen  within  the  favorable  orientation  range  to  guarantee  a 
interference-free  motion. 

The  efforts  of  this  work  are  summarized  in  the  followings. 

(1)  The  determination  of  orientation  ranges  of  links  of  a robot  manipulator 
avoiding  spherical  obstacles  while  its  end  effector  follows  a prespecified  path  is 
shown. 

(2)  The  determination  of  orientation  ranges  of  links  of  a robot  manipulator 
avoiding  polyhedral  obstacles  while  its  end  effector  follows  a prespecified  path  is 
presented. 
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(3)  The  determination  of  orientation  ranges  of  links  of  a robot  manipulator 
avoiding  the  links  of  a second  robot  manipulator  while  their  end  effectors  follow 
prespecified  trajectories  is  demonstrated. 

(4)  Trajectories  planning  of  two  robot  manipulators  using  time  scheduling 
scheme  such  that  the  two  manipulators  do  not  collide  with  each  other  is  illustrated. 

This  study  provides  an  understanding  of  dexterity  of  robot  manipulators 
subject  to  constraint  due  to  the  presence  of  obstacles.  It  also  provides  a decision- 
making tool  for  off-line  motion  planning  of  spatial  robot  manipulators.  When  a path 
for  a spatial  robot  is  designated,  the  analysis  in  this  work  determines  whether  the 
robot  is  capable  of  tracking  this  path  and  how  to  control  the  robot  to  track  the  path. 
Further,  the  time  scheduling  schemes  for  the  coordinations  of  two  robot 
manipulators  provide  an  effective  way  for  planning  the  trajectories  of  the  two 
manipulators  to  avoid  interference.  Finally,  it  is  worth  to  mention  that  the  joint 
displacement  limitations  of  robots  can  be  easily  implemented  into  the  analysis  by 
reducing  the  permissible  orientation  ranges  of  robot  links. 

This  research  may  also  find  applications  for  the  motion  planning  of 
kinematically  redundant  robots  which  can  be  divided  into  non-redundant  segments 
with  ends  constrained  by  joints.  The  motion  capabilities  of  links  of  the  robot  in  each 
segment  can  then  be  determined  by  the  method  presented  in  this  work. 

In  this  research,  the  problem  of  the  trajectories  planning  for  the  coordination 
of  two  robot  manipulators  is  discussed  where  each  robot  is  assumed  to  have  a 
trajectory  with  acceleration  and  deceleration  of  the  same  magnitude.  This  can  be 
extended  to  the  problem  where  the  robot  has  different  magnitudes  of  acceleration 
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and  deceleration.  Further,  the  problem  can  be  extended  to  consider  alternative 
tactics  such  as  changing  the  path  of  one  robot  or  altering  the  trajectories  of  both 
robots.  It  should  be  remembered  that  in  the  trajectory  planning  analysis,  the 
collision  is  considered  to  occur  only  between  the  end  effectors  of  the  two  robots. 
The  cases  where  other  links  of  the  robots  may  involve  in  the  collision  worth  further 
investigation.  Finally,  this  analysis  can  be  applied  to  the  collision  avoidance  of 
mobile  robots  or  automatically  guided  vehicles.  In  this  case,  the  trajecory  of  a robot 
or  vehicle  is  planned  according  to  the  relative  motions  of  other  robots  or  vehicles. 
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